Merge remote-tracking branch 'upstream/master'

update to original remote repo

.gitignore

@@ -34,3 +34,6 @@ __pycache__
 
 # JetBrains IDE
 .idea/
+
+# python
+dist/

README.md

@@ -8,6 +8,12 @@ Python interface for unitree sdk2
 - numpy
 - opencv-python
 ## Install unitree_sdk2_python
+
+```bash
+pip install unitree_sdk2py
+```
+
+### Installing from source
 Execute the following commands in the terminal:
 ```bash
 cd ~

example/b2/camera/camera_opencv.py

@@ -0,0 +1,51 @@
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient
+from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient
+import cv2
+import numpy as np
+import sys
+
+def display_image(window_name, data):
+    # If data is a list, we need to convert it to a bytes object
+    if isinstance(data, list):
+        data = bytes(data)
+    
+    # Now convert to numpy image
+    image_data = np.frombuffer(data, dtype=np.uint8)
+    image = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
+    if image is not None:
+        cv2.imshow(window_name, image)
+
+if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    frontCameraClient = FrontVideoClient()  # Create a front camera video client
+    frontCameraClient.SetTimeout(3.0)
+    frontCameraClient.Init()
+
+    backCameraClient = BackVideoClient()  # Create a back camera video client
+    backCameraClient.SetTimeout(3.0)
+    backCameraClient.Init()
+
+    # Loop to continuously fetch images
+    while True:
+        # Get front camera image
+        front_code, front_data = frontCameraClient.GetImageSample()
+        if front_code == 0:
+            display_image("Front Camera", front_data)
+
+        # Get back camera image
+        back_code, back_data = backCameraClient.GetImageSample()
+        if back_code == 0:
+            display_image("Back Camera", back_data)
+
+        # Press ESC to stop
+        if cv2.waitKey(20) == 27:
+            break
+
+    # Clean up windows
+    cv2.destroyAllWindows()
+

example/b2/camera/capture_image.py

@@ -0,0 +1,51 @@
+import time
+import os
+import sys
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient
+from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient
+
+if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    # 创建前置相机客户端
+    front_client = FrontVideoClient()
+    front_client.SetTimeout(3.0)
+    front_client.Init()
+
+    # 创建后置相机客户端
+    back_client = BackVideoClient()
+    back_client.SetTimeout(3.0)
+    back_client.Init()
+
+    print("##################Get Front Camera Image###################")
+    # 获取前置相机图像
+    front_code, front_data = front_client.GetImageSample()
+
+    if front_code != 0:
+        print("Get front camera image error. Code:", front_code)
+    else:
+        front_image_name = "./front_img.jpg"
+        print("Front Image Saved as:", front_image_name)
+
+        with open(front_image_name, "+wb") as f:
+            f.write(bytes(front_data))
+
+    print("##################Get Back Camera Image###################")
+    # 获取后置相机图像
+    back_code, back_data = back_client.GetImageSample()
+
+    if back_code != 0:
+        print("Get back camera image error. Code:", back_code)
+    else:
+        back_image_name = "./back_img.jpg"
+        print("Back Image Saved as:", back_image_name)
+
+        with open(back_image_name, "+wb") as f:
+            f.write(bytes(back_data))
+
+    time.sleep(1)

example/b2/high_level/b2_sport_client.py

@@ -0,0 +1,105 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.b2.sport.sport_client import SportClient
+import math
+from dataclasses import dataclass
+
+@dataclass
+class TestOption:
+    name: str
+    id: int
+
+option_list = [
+    TestOption(name="damp", id=0),         
+    TestOption(name="stand_up", id=1),     
+    TestOption(name="stand_down", id=2),   
+    TestOption(name="move forward", id=3),         
+    TestOption(name="move lateral", id=4),    
+    TestOption(name="move rotate", id=5),  
+    TestOption(name="stop_move", id=6),  
+    TestOption(name="switch_gait", id=7),    
+    TestOption(name="switch_gait", id=8),     
+    TestOption(name="recovery", id=9),
+    TestOption(name="balanced stand", id=10)       
+]
+
+class UserInterface:
+    def __init__(self):
+        self.test_option_ = None
+
+    def convert_to_int(self, input_str):
+        try:
+            return int(input_str)
+        except ValueError:
+            return None
+
+    def terminal_handle(self):
+        input_str = input("Enter id or name: \n")
+
+        if input_str == "list":
+            self.test_option_.name = None
+            self.test_option_.id = None
+            for option in option_list:
+                print(f"{option.name}, id: {option.id}")
+            return
+
+        for option in option_list:
+            if input_str == option.name or self.convert_to_int(input_str) == option.id:
+                self.test_option_.name = option.name
+                self.test_option_.id = option.id
+                print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
+                return
+
+        print("No matching test option found.")
+
+if __name__ == "__main__":
+
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} networkInterface")
+        sys.exit(-1)
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    test_option = TestOption(name=None, id=None) 
+    user_interface = UserInterface()
+    user_interface.test_option_ = test_option
+
+    sport_client = SportClient()  
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+
+    print("Input \"list\" to list all test option ...")
+
+    while True:
+        user_interface.terminal_handle()
+
+        print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}")
+
+        if test_option.id == 0:
+            sport_client.Damp()
+        elif test_option.id == 1:
+            sport_client.StandUp()
+        elif test_option.id == 2:
+            sport_client.StandDown()
+        elif test_option.id == 3:
+            sport_client.Move(0.3,0,0)
+        elif test_option.id == 4:
+            sport_client.Move(0,0.3,0)
+        elif test_option.id == 5:
+            sport_client.Move(0,0,0.5)
+        elif test_option.id == 6:
+            sport_client.StopMove()
+        elif test_option.id == 7:
+            sport_client.SwitchGait(0)
+        elif test_option.id == 8:
+            sport_client.SwitchGait(1)
+        elif test_option.id == 9:
+            sport_client.RecoveryStand()
+        elif test_option.id == 10:
+            sport_client.BalanceStand()
+
+        time.sleep(1)

example/b2/low_level/b2_stand_example.py

@@ -0,0 +1,175 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+from unitree_sdk2py.utils.thread import RecurrentThread
+import unitree_legged_const as b2
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+from unitree_sdk2py.b2.sport.sport_client import SportClient
+
+from unitree_sdk2py.utils.crc import CRC
+
+class Custom:
+    def __init__(self):
+        self.Kp = 1000.0
+        self.Kd = 10.0
+        self.time_consume = 0
+        self.rate_count = 0
+        self.sin_count = 0
+        self.motiontime = 0
+        self.dt = 0.002 
+
+        self.low_cmd = unitree_go_msg_dds__LowCmd_()  
+        self.low_state = None  
+
+        self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                             -0.2, 1.36, -2.65, 0.2, 1.36, -2.65]
+
+        self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                             0.0, 0.67, -1.3, 0.0, 0.67, -1.3]
+
+        self.targetPos_3 = [-0.5, 1.36, -2.65, 0.5, 1.36, -2.65,
+                             -0.5, 1.36, -2.65, 0.5, 1.36, -2.65]
+
+        self.startPos = [0.0] * 12
+        self.duration_1 = 500
+        self.duration_2 = 900
+        self.duration_3 = 1000
+        self.duration_4 = 900
+        self.percent_1 = 0
+        self.percent_2 = 0
+        self.percent_3 = 0
+        self.percent_4 = 0
+
+        self.firstRun = True
+        self.done = False
+
+        # thread handling
+        self.lowCmdWriteThreadPtr = None
+
+        self.crc = CRC()
+
+    def Init(self):
+        self.InitLowCmd()
+
+        # create publisher #
+        self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
+
+        self.sc = SportClient()  
+        self.sc.SetTimeout(5.0)
+        self.sc.Init()
+
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.sc.StandDown()
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=0.002, target=self.LowCmdWrite, name="writebasiccmd"
+        )
+        self.lowCmdWriteThreadPtr.Start()
+
+    def InitLowCmd(self):
+        self.low_cmd.head[0] = 0xFE
+        self.low_cmd.head[1] = 0xEF
+        self.low_cmd.level_flag = 0xFF
+        self.low_cmd.gpio = 0
+        for i in range(20):
+            self.low_cmd.motor_cmd[i].mode = 0x01 
+            self.low_cmd.motor_cmd[i].q= b2.PosStopF
+            self.low_cmd.motor_cmd[i].kp = 0
+            self.low_cmd.motor_cmd[i].dq = b2.VelStopF
+            self.low_cmd.motor_cmd[i].kd = 0
+            self.low_cmd.motor_cmd[i].tau = 0
+
+    def LowStateMessageHandler(self, msg: LowState_):
+        self.low_state = msg
+
+    def LowCmdWrite(self):
+
+        if self.firstRun:
+            for i in range(12):
+                self.startPos[i] = self.low_state.motor_state[i].q
+            self.firstRun = False
+
+        self.percent_1 += 1.0 / self.duration_1
+        self.percent_1 = min(self.percent_1, 1)
+        if self.percent_1 < 1:
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 <= 1):
+            self.percent_2 += 1.0 / self.duration_2
+            self.percent_2 = min(self.percent_2, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1):
+            self.percent_3 += 1.0 / self.duration_3
+            self.percent_3 = min(self.percent_3, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = self.targetPos_2[i] 
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1):
+            self.percent_4 += 1.0 / self.duration_4
+            self.percent_4 = min(self.percent_4, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:   
+        if custom.percent_4 == 1.0: 
+           time.sleep(1)
+           print("Done!")
+           sys.exit(-1)     
+        time.sleep(1)

example/b2w/camera/camera_opencv.py

@@ -0,0 +1,51 @@
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient
+from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient
+import cv2
+import numpy as np
+import sys
+
+def display_image(window_name, data):
+    # If data is a list, we need to convert it to a bytes object
+    if isinstance(data, list):
+        data = bytes(data)
+    
+    # Now convert to numpy image
+    image_data = np.frombuffer(data, dtype=np.uint8)
+    image = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
+    if image is not None:
+        cv2.imshow(window_name, image)
+
+if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    frontCameraClient = FrontVideoClient()  # Create a front camera video client
+    frontCameraClient.SetTimeout(3.0)
+    frontCameraClient.Init()
+
+    backCameraClient = BackVideoClient()  # Create a back camera video client
+    backCameraClient.SetTimeout(3.0)
+    backCameraClient.Init()
+
+    # Loop to continuously fetch images
+    while True:
+        # Get front camera image
+        front_code, front_data = frontCameraClient.GetImageSample()
+        if front_code == 0:
+            display_image("Front Camera", front_data)
+
+        # Get back camera image
+        back_code, back_data = backCameraClient.GetImageSample()
+        if back_code == 0:
+            display_image("Back Camera", back_data)
+
+        # Press ESC to stop
+        if cv2.waitKey(20) == 27:
+            break
+
+    # Clean up windows
+    cv2.destroyAllWindows()
+

example/b2w/camera/capture_image.py

@@ -0,0 +1,51 @@
+import time
+import os
+import sys
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.b2.front_video.front_video_client import FrontVideoClient
+from unitree_sdk2py.b2.back_video.back_video_client import BackVideoClient
+
+if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    # 创建前置相机客户端
+    front_client = FrontVideoClient()
+    front_client.SetTimeout(3.0)
+    front_client.Init()
+
+    # 创建后置相机客户端
+    back_client = BackVideoClient()
+    back_client.SetTimeout(3.0)
+    back_client.Init()
+
+    print("##################Get Front Camera Image###################")
+    # 获取前置相机图像
+    front_code, front_data = front_client.GetImageSample()
+
+    if front_code != 0:
+        print("Get front camera image error. Code:", front_code)
+    else:
+        front_image_name = "./front_img.jpg"
+        print("Front Image Saved as:", front_image_name)
+
+        with open(front_image_name, "+wb") as f:
+            f.write(bytes(front_data))
+
+    print("##################Get Back Camera Image###################")
+    # 获取后置相机图像
+    back_code, back_data = back_client.GetImageSample()
+
+    if back_code != 0:
+        print("Get back camera image error. Code:", back_code)
+    else:
+        back_image_name = "./back_img.jpg"
+        print("Back Image Saved as:", back_image_name)
+
+        with open(back_image_name, "+wb") as f:
+            f.write(bytes(back_data))
+
+    time.sleep(1)

example/b2w/high_level/b2w_sport_client.py

@@ -0,0 +1,101 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.b2.sport.sport_client import SportClient
+import math
+from dataclasses import dataclass
+
+@dataclass
+class TestOption:
+    name: str
+    id: int
+
+option_list = [
+    TestOption(name="damp", id=0),         
+    TestOption(name="stand_up", id=1),     
+    TestOption(name="stand_down", id=2),   
+    TestOption(name="move forward", id=3),         
+    TestOption(name="move lateral", id=4),    
+    TestOption(name="move rotate", id=5),  
+    TestOption(name="stop_move", id=6),  
+    TestOption(name="switch_gait", id=7),    
+    TestOption(name="switch_gait", id=8),     
+    TestOption(name="recovery", id=9)       
+]
+
+class UserInterface:
+    def __init__(self):
+        self.test_option_ = None
+
+    def convert_to_int(self, input_str):
+        try:
+            return int(input_str)
+        except ValueError:
+            return None
+
+    def terminal_handle(self):
+        input_str = input("Enter id or name: \n")
+
+        if input_str == "list":
+            self.test_option_.name = None
+            self.test_option_.id = None
+            for option in option_list:
+                print(f"name: {option.name}, id: {option.id}")
+            return
+
+        for option in option_list:
+            if input_str == option.name or self.convert_to_int(input_str) == option.id:
+                self.test_option_.name = option.name
+                self.test_option_.id = option.id
+                print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
+                return
+
+        print("No matching test option found.")
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} networkInterface")
+        sys.exit(-1)
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    test_option = TestOption(name=None, id=None)  
+    user_interface = UserInterface()
+    user_interface.test_option_ = test_option
+
+    sport_client = SportClient() 
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+
+    print("Input \"list\" to list all test option ...")
+
+    while True:
+        user_interface.terminal_handle()
+
+        print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n")
+
+        if test_option.id == 0:
+            sport_client.Damp()
+        elif test_option.id == 1:
+            sport_client.StandUp()
+        elif test_option.id == 2:
+            sport_client.StandDown()
+        elif test_option.id == 3:
+            sport_client.Move(0.3,0,0)
+        elif test_option.id == 4:
+            sport_client.Move(0,0.3,0)
+        elif test_option.id == 5:
+            sport_client.Move(0,0,0.5)
+        elif test_option.id == 6:
+            sport_client.StopMove()
+        elif test_option.id == 7:
+            sport_client.SwitchGait(0)
+        elif test_option.id == 8:
+            sport_client.SwitchGait(1)
+        elif test_option.id == 9:
+            sport_client.RecoveryStand()
+
+        time.sleep(1)

example/b2w/low_level/b2w_stand_example.py

@@ -0,0 +1,196 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+import unitree_legged_const as b2w
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+from unitree_sdk2py.b2.sport.sport_client import SportClient
+
+class Custom:
+    def __init__(self):
+        self.Kp = 1000.0
+        self.Kd = 10.0
+        self.time_consume = 0
+        self.rate_count = 0
+        self.sin_count = 0
+        self.motiontime = 0
+        self.dt = 0.002  
+
+        self.low_cmd = unitree_go_msg_dds__LowCmd_()  
+        self.low_state = None  
+
+        self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                             -0.2, 1.36, -2.65, 0.2, 1.36, -2.65]
+
+        self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                             0.0, 0.67, -1.3, 0.0, 0.67, -1.3]
+
+        self.targetPos_3 = [-0.65, 1.36, -2.65, 0.65, 1.36, -2.65,
+                             -0.65, 1.36, -2.65, 0.65, 1.36, -2.65]
+
+        self.startPos = [0.0] * 12
+        self.duration_1 = 800
+        self.duration_2 = 800
+        self.duration_3 = 2000
+        self.duration_4 = 1500
+        self.percent_1 = 0
+        self.percent_2 = 0
+        self.percent_3 = 0
+        self.percent_4 = 0
+
+        self.firstRun = True
+        self.done = False
+
+        # thread handling
+        self.lowCmdWriteThreadPtr = None
+
+        self.crc = CRC()
+
+    def Init(self):
+        self.InitLowCmd()
+
+        # create publisher #
+        self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
+
+        self.sc = SportClient()  
+        self.sc.SetTimeout(5.0)
+        self.sc.Init()
+
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.sc.StandDown()
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            name="writebasiccmd", interval=self.dt, target=self.LowCmdWrite, 
+        )
+        self.lowCmdWriteThreadPtr.Start()
+
+    def InitLowCmd(self):
+        self.low_cmd.head[0] = 0xFE
+        self.low_cmd.head[1] = 0xEF
+        self.low_cmd.level_flag = 0xFF
+        self.low_cmd.gpio = 0
+        for i in range(20):
+            self.low_cmd.motor_cmd[i].mode = 0x01  # (PMSM) mode
+            self.low_cmd.motor_cmd[i].q = b2w.PosStopF
+            self.low_cmd.motor_cmd[i].kp = 0
+            self.low_cmd.motor_cmd[i].dq = b2w.VelStopF
+            self.low_cmd.motor_cmd[i].kd = 0
+            self.low_cmd.motor_cmd[i].tau = 0
+
+    def LowStateMessageHandler(self, msg: LowState_):
+        self.low_state = msg
+
+    def LowCmdWrite(self):
+        if self.firstRun:
+            for i in range(12):
+                self.startPos[i] = self.low_state.motor_state[i].q
+            self.firstRun = False
+
+        self.percent_1 += 1.0 / self.duration_1
+        self.percent_1 = min(self.percent_1, 1)
+        if self.percent_1 < 1:
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 <= 1):
+            self.percent_2 += 1.0 / self.duration_2
+            self.percent_2 = min(self.percent_2, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1):
+            self.percent_3 += 1.0 / self.duration_3
+            self.percent_3 = min(self.percent_3, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = self.targetPos_2[i] 
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+            if self.percent_3 < 0.4:
+                for i in range(12, 16):
+                    self.low_cmd.motor_cmd[i].q = 0 
+                    self.low_cmd.motor_cmd[i].kp = 0
+                    self.low_cmd.motor_cmd[i].dq = 3
+                    self.low_cmd.motor_cmd[i].kd = self.Kd
+                    self.low_cmd.motor_cmd[i].tau = 0
+
+            if 0.4 <= self.percent_3 < 0.8:
+                for i in range(12, 16):
+                    self.low_cmd.motor_cmd[i].q = 0 
+                    self.low_cmd.motor_cmd[i].kp = 0
+                    self.low_cmd.motor_cmd[i].dq = -3
+                    self.low_cmd.motor_cmd[i].kd = self.Kd
+                    self.low_cmd.motor_cmd[i].tau = 0
+
+            if self.percent_3 >= 0.8:
+                for i in range(12, 16):
+                    self.low_cmd.motor_cmd[i].q = 0 
+                    self.low_cmd.motor_cmd[i].kp = 0
+                    self.low_cmd.motor_cmd[i].dq = 0
+                    self.low_cmd.motor_cmd[i].kd = self.Kd
+                    self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1):
+            self.percent_4 += 1.0 / self.duration_4
+            self.percent_4 = min(self.percent_4, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:   
+        if custom.percent_4 == 1.0: 
+           time.sleep(1)
+           print("Done!")
+           sys.exit(-1)     
+        time.sleep(1)

example/b2w/low_level/unitree_legged_const.py

@@ -0,0 +1,24 @@
+LegID = {
+    "FR_0": 0,  # Front right hip
+    "FR_1": 1,  # Front right thigh
+    "FR_2": 2,  # Front right calf
+    "FL_0": 3,
+    "FL_1": 4,
+    "FL_2": 5,
+    "RR_0": 6,
+    "RR_1": 7,
+    "RR_2": 8,
+    "RL_0": 9,
+    "RL_1": 10,
+    "RL_2": 11,
+    "FR_w": 12, # Front right wheel
+    "FL_w": 13, # Front left wheel
+    "RR_w": 14, # Rear right wheel
+    "RL_w": 15, # Rear left wheel
+}
+
+HIGHLEVEL = 0xEE
+LOWLEVEL = 0xFF
+TRIGERLEVEL = 0xF0
+PosStopF = 2.146e9
+VelStopF = 16000.0

example/g1/audio/g1_audio_client_example.py

@@ -0,0 +1,44 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient
+from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} networkInterface")
+        sys.exit(-1)
+
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    audio_client = AudioClient()  
+    audio_client.SetTimeout(10.0)
+    audio_client.Init()
+
+    sport_client = LocoClient()  
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+
+    ret = audio_client.GetVolume()
+    print("debug GetVolume: ",ret)
+
+    audio_client.SetVolume(85)
+
+    ret = audio_client.GetVolume()
+    print("debug GetVolume: ",ret)
+
+    sport_client.WaveHand()
+
+    audio_client.TtsMaker("大家好!我是宇树科技人形机器人。语音开发测试例程运行成功！ 很高兴认识你！",0)
+    time.sleep(8)
+    audio_client.TtsMaker("接下来测试灯带开发例程！",0)
+    time.sleep(1)
+    audio_client.LedControl(255,0,0)
+    time.sleep(1)
+    audio_client.LedControl(0,255,0)
+    time.sleep(1)
+    audio_client.LedControl(0,0,255)
+
+    time.sleep(3)
+    audio_client.TtsMaker("测试完毕，谢谢大家！",0)
+    

example/g1/high_level/g1_arm5_sdk_dds_example.py

@@ -0,0 +1,192 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+import numpy as np
+
+kPi = 3.141592654
+kPi_2 = 1.57079632
+
+class G1JointIndex:
+    # Left leg
+    LeftHipPitch = 0
+    LeftHipRoll = 1
+    LeftHipYaw = 2
+    LeftKnee = 3
+    LeftAnklePitch = 4
+    LeftAnkleB = 4
+    LeftAnkleRoll = 5
+    LeftAnkleA = 5
+
+    # Right leg
+    RightHipPitch = 6
+    RightHipRoll = 7
+    RightHipYaw = 8
+    RightKnee = 9
+    RightAnklePitch = 10
+    RightAnkleB = 10
+    RightAnkleRoll = 11
+    RightAnkleA = 11
+
+    WaistYaw = 12
+    WaistRoll = 13        # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistA = 13           # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistPitch = 14       # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistB = 14           # NOTE: INVALID for g1 23dof/29dof with waist locked
+
+    # Left arm
+    LeftShoulderPitch = 15
+    LeftShoulderRoll = 16
+    LeftShoulderYaw = 17
+    LeftElbow = 18
+    LeftWristRoll = 19
+    LeftWristPitch = 20   # NOTE: INVALID for g1 23dof
+    LeftWristYaw = 21     # NOTE: INVALID for g1 23dof
+
+    # Right arm
+    RightShoulderPitch = 22
+    RightShoulderRoll = 23
+    RightShoulderYaw = 24
+    RightElbow = 25
+    RightWristRoll = 26
+    RightWristPitch = 27  # NOTE: INVALID for g1 23dof
+    RightWristYaw = 28    # NOTE: INVALID for g1 23dof
+
+    kNotUsedJoint = 29 # NOTE: Weight
+
+class Custom:
+    def __init__(self):
+        self.time_ = 0.0
+        self.control_dt_ = 0.02  
+        self.duration_ = 3.0   
+        self.counter_ = 0
+        self.weight = 0.
+        self.weight_rate = 0.2
+        self.kp = 60.
+        self.kd = 1.5
+        self.dq = 0.
+        self.tau_ff = 0.
+        self.mode_machine_ = 0
+        self.low_cmd = unitree_hg_msg_dds__LowCmd_()  
+        self.low_state = None 
+        self.first_update_low_state = False
+        self.crc = CRC()
+        self.done = False
+
+        self.target_pos = [
+            0.0,      kPi_2,  0.0,    kPi_2,  0.0,
+            0.0,     -kPi_2,  0.0,    kPi_2,  0.0,
+            0.0,      0.0,    0.0
+        ]
+
+        self.arm_joints = [
+          G1JointIndex.LeftShoulderPitch,  G1JointIndex.LeftShoulderRoll,
+          G1JointIndex.LeftShoulderYaw,    G1JointIndex.LeftElbow,
+          G1JointIndex.LeftWristRoll,
+          G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll,
+          G1JointIndex.RightShoulderYaw,   G1JointIndex.RightElbow,
+          G1JointIndex.RightWristRoll,
+          G1JointIndex.WaistYaw,
+          G1JointIndex.WaistRoll,
+          G1JointIndex.WaistPitch
+        ]
+
+    def Init(self):
+        # create publisher #
+        self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_)
+        self.arm_sdk_publisher.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateHandler, 10)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=self.control_dt_, target=self.LowCmdWrite, name="control"
+        )
+        while self.first_update_low_state == False:
+            time.sleep(1)
+
+        if self.first_update_low_state == True:
+            self.lowCmdWriteThreadPtr.Start()
+
+    def LowStateHandler(self, msg: LowState_):
+        self.low_state = msg
+
+        if self.first_update_low_state == False:
+            self.first_update_low_state = True
+        
+    def LowCmdWrite(self):
+        self.time_ += self.control_dt_
+
+        if self.time_ < self.duration_ :
+          # [Stage 1]: set robot to zero posture
+          self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q =  1 # 1:Enable arm_sdk, 0:Disable arm_sdk
+          for i,joint in enumerate(self.arm_joints):
+            ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
+            self.low_cmd.motor_cmd[joint].tau = 0. 
+            self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q 
+            self.low_cmd.motor_cmd[joint].dq = 0. 
+            self.low_cmd.motor_cmd[joint].kp = self.kp 
+            self.low_cmd.motor_cmd[joint].kd = self.kd
+
+        elif self.time_ < self.duration_ * 3 :
+          # [Stage 2]: lift arms up
+          for i,joint in enumerate(self.arm_joints):
+              ratio = np.clip((self.time_ - self.duration_) / (self.duration_ * 2), 0.0, 1.0)
+              self.low_cmd.motor_cmd[joint].tau = 0. 
+              self.low_cmd.motor_cmd[joint].q = ratio * self.target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q 
+              self.low_cmd.motor_cmd[joint].dq = 0. 
+              self.low_cmd.motor_cmd[joint].kp = self.kp 
+              self.low_cmd.motor_cmd[joint].kd = self.kd
+
+        elif self.time_ < self.duration_ * 6 :
+          # [Stage 3]: set robot back to zero posture
+          for i,joint in enumerate(self.arm_joints):
+              ratio = np.clip((self.time_ - self.duration_*3) / (self.duration_ * 3), 0.0, 1.0)
+              self.low_cmd.motor_cmd[joint].tau = 0. 
+              self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q
+              self.low_cmd.motor_cmd[joint].dq = 0. 
+              self.low_cmd.motor_cmd[joint].kp = self.kp 
+              self.low_cmd.motor_cmd[joint].kd = self.kd
+
+        elif self.time_ < self.duration_ * 7 :
+          # [Stage 4]: release arm_sdk
+          for i,joint in enumerate(self.arm_joints):
+              ratio = np.clip((self.time_ - self.duration_*6) / (self.duration_), 0.0, 1.0)
+              self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q =  (1 - ratio) # 1:Enable arm_sdk, 0:Disable arm_sdk
+        
+        else:
+            self.done = True
+  
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.arm_sdk_publisher.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:        
+        time.sleep(1)
+        if custom.done: 
+           print("Done!")
+           sys.exit(-1)     

example/g1/high_level/g1_arm7_sdk_dds_example.py

@@ -0,0 +1,194 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+import numpy as np
+
+kPi = 3.141592654
+kPi_2 = 1.57079632
+
+class G1JointIndex:
+    # Left leg
+    LeftHipPitch = 0
+    LeftHipRoll = 1
+    LeftHipYaw = 2
+    LeftKnee = 3
+    LeftAnklePitch = 4
+    LeftAnkleB = 4
+    LeftAnkleRoll = 5
+    LeftAnkleA = 5
+
+    # Right leg
+    RightHipPitch = 6
+    RightHipRoll = 7
+    RightHipYaw = 8
+    RightKnee = 9
+    RightAnklePitch = 10
+    RightAnkleB = 10
+    RightAnkleRoll = 11
+    RightAnkleA = 11
+
+    WaistYaw = 12
+    WaistRoll = 13        # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistA = 13           # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistPitch = 14       # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistB = 14           # NOTE: INVALID for g1 23dof/29dof with waist locked
+
+    # Left arm
+    LeftShoulderPitch = 15
+    LeftShoulderRoll = 16
+    LeftShoulderYaw = 17
+    LeftElbow = 18
+    LeftWristRoll = 19
+    LeftWristPitch = 20   # NOTE: INVALID for g1 23dof
+    LeftWristYaw = 21     # NOTE: INVALID for g1 23dof
+
+    # Right arm
+    RightShoulderPitch = 22
+    RightShoulderRoll = 23
+    RightShoulderYaw = 24
+    RightElbow = 25
+    RightWristRoll = 26
+    RightWristPitch = 27  # NOTE: INVALID for g1 23dof
+    RightWristYaw = 28    # NOTE: INVALID for g1 23dof
+
+    kNotUsedJoint = 29 # NOTE: Weight
+
+class Custom:
+    def __init__(self):
+        self.time_ = 0.0
+        self.control_dt_ = 0.02  
+        self.duration_ = 3.0   
+        self.counter_ = 0
+        self.weight = 0.
+        self.weight_rate = 0.2
+        self.kp = 60.
+        self.kd = 1.5
+        self.dq = 0.
+        self.tau_ff = 0.
+        self.mode_machine_ = 0
+        self.low_cmd = unitree_hg_msg_dds__LowCmd_()  
+        self.low_state = None 
+        self.first_update_low_state = False
+        self.crc = CRC()
+        self.done = False
+
+        self.target_pos = [
+            0., kPi_2,  0., kPi_2, 0., 0., 0.,
+            0., -kPi_2, 0., kPi_2, 0., 0., 0., 
+            0, 0, 0
+        ]
+
+        self.arm_joints = [
+          G1JointIndex.LeftShoulderPitch,  G1JointIndex.LeftShoulderRoll,
+          G1JointIndex.LeftShoulderYaw,    G1JointIndex.LeftElbow,
+          G1JointIndex.LeftWristRoll,      G1JointIndex.LeftWristPitch,
+          G1JointIndex.LeftWristYaw,
+          G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll,
+          G1JointIndex.RightShoulderYaw,   G1JointIndex.RightElbow,
+          G1JointIndex.RightWristRoll,     G1JointIndex.RightWristPitch,
+          G1JointIndex.RightWristYaw,
+          G1JointIndex.WaistYaw,
+          G1JointIndex.WaistRoll,
+          G1JointIndex.WaistPitch
+        ]
+
+    def Init(self):
+        # create publisher #
+        self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_)
+        self.arm_sdk_publisher.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateHandler, 10)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=self.control_dt_, target=self.LowCmdWrite, name="control"
+        )
+        while self.first_update_low_state == False:
+            time.sleep(1)
+
+        if self.first_update_low_state == True:
+            self.lowCmdWriteThreadPtr.Start()
+
+    def LowStateHandler(self, msg: LowState_):
+        self.low_state = msg
+
+        if self.first_update_low_state == False:
+            self.first_update_low_state = True
+        
+    def LowCmdWrite(self):
+        self.time_ += self.control_dt_
+
+        if self.time_ < self.duration_ :
+          # [Stage 1]: set robot to zero posture
+          self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q =  1 # 1:Enable arm_sdk, 0:Disable arm_sdk
+          for i,joint in enumerate(self.arm_joints):
+            ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
+            self.low_cmd.motor_cmd[joint].tau = 0. 
+            self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q 
+            self.low_cmd.motor_cmd[joint].dq = 0. 
+            self.low_cmd.motor_cmd[joint].kp = self.kp 
+            self.low_cmd.motor_cmd[joint].kd = self.kd
+
+        elif self.time_ < self.duration_ * 3 :
+          # [Stage 2]: lift arms up
+          for i,joint in enumerate(self.arm_joints):
+              ratio = np.clip((self.time_ - self.duration_) / (self.duration_ * 2), 0.0, 1.0)
+              self.low_cmd.motor_cmd[joint].tau = 0. 
+              self.low_cmd.motor_cmd[joint].q = ratio * self.target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q 
+              self.low_cmd.motor_cmd[joint].dq = 0. 
+              self.low_cmd.motor_cmd[joint].kp = self.kp 
+              self.low_cmd.motor_cmd[joint].kd = self.kd
+
+        elif self.time_ < self.duration_ * 6 :
+          # [Stage 3]: set robot back to zero posture
+          for i,joint in enumerate(self.arm_joints):
+              ratio = np.clip((self.time_ - self.duration_*3) / (self.duration_ * 3), 0.0, 1.0)
+              self.low_cmd.motor_cmd[joint].tau = 0. 
+              self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q
+              self.low_cmd.motor_cmd[joint].dq = 0. 
+              self.low_cmd.motor_cmd[joint].kp = self.kp 
+              self.low_cmd.motor_cmd[joint].kd = self.kd
+
+        elif self.time_ < self.duration_ * 7 :
+          # [Stage 4]: release arm_sdk
+          for i,joint in enumerate(self.arm_joints):
+              ratio = np.clip((self.time_ - self.duration_*6) / (self.duration_), 0.0, 1.0)
+              self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q =  (1 - ratio) # 1:Enable arm_sdk, 0:Disable arm_sdk
+        
+        else:
+            self.done = True
+  
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.arm_sdk_publisher.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:        
+        time.sleep(1)
+        if custom.done: 
+           print("Done!")
+           sys.exit(-1)    

example/g1/high_level/g1_loco_client_example.py

@@ -0,0 +1,110 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
+from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+import math
+from dataclasses import dataclass
+
+@dataclass
+class TestOption:
+    name: str
+    id: int
+
+option_list = [
+    TestOption(name="damp", id=0),         
+    TestOption(name="stand_up", id=1),     
+    TestOption(name="sit", id=2),   
+    TestOption(name="move forward", id=3),         
+    TestOption(name="move lateral", id=4),    
+    TestOption(name="move rotate", id=5),  
+    TestOption(name="low stand", id=6),  
+    TestOption(name="high stand", id=7),    
+    TestOption(name="zero torque", id=8),
+    TestOption(name="wave hand1", id=9), # wave hand without turning around
+    TestOption(name="wave hand2", id=10), # wave hand and trun around  
+    TestOption(name="shake hand", id=11),     
+]
+
+class UserInterface:
+    def __init__(self):
+        self.test_option_ = None
+
+    def convert_to_int(self, input_str):
+        try:
+            return int(input_str)
+        except ValueError:
+            return None
+
+    def terminal_handle(self):
+        input_str = input("Enter id or name: \n")
+
+        if input_str == "list":
+            self.test_option_.name = None
+            self.test_option_.id = None
+            for option in option_list:
+                print(f"{option.name}, id: {option.id}")
+            return
+
+        for option in option_list:
+            if input_str == option.name or self.convert_to_int(input_str) == option.id:
+                self.test_option_.name = option.name
+                self.test_option_.id = option.id
+                print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
+                return
+
+        print("No matching test option found.")
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} networkInterface")
+        sys.exit(-1)
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    test_option = TestOption(name=None, id=None) 
+    user_interface = UserInterface()
+    user_interface.test_option_ = test_option
+
+    sport_client = LocoClient()  
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+
+    print("Input \"list\" to list all test option ...")
+    while True:
+        user_interface.terminal_handle()
+
+        print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}")
+
+        if test_option.id == 0:
+            sport_client.Damp()
+        elif test_option.id == 1:
+            sport_client.StandUp()
+        elif test_option.id == 2:
+            sport_client.Sit()
+        elif test_option.id == 3:
+            sport_client.Move(0.3,0,0)
+        elif test_option.id == 4:
+            sport_client.Move(0,0.3,0)
+        elif test_option.id == 5:
+            sport_client.Move(0,0,0.3)
+        elif test_option.id == 6:
+            sport_client.LowStand()
+        elif test_option.id == 7:
+            sport_client.HighStand()
+        elif test_option.id == 8:
+            sport_client.ZeroTorque()
+        elif test_option.id == 9:
+            sport_client.WaveHand()
+        elif test_option.id == 10:
+            sport_client.WaveHand(True)
+        elif test_option.id == 11:
+            sport_client.ShakeHand()
+            time.sleep(3)
+            sport_client.ShakeHand()
+
+        time.sleep(1)

example/g1/low_level/g1_low_level_example.py

@@ -0,0 +1,205 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+import numpy as np
+
+G1_NUM_MOTOR = 29
+
+Kp = [
+    60, 60, 60, 100, 40, 40,      # legs
+    60, 60, 60, 100, 40, 40,      # legs
+    60, 40, 40,                   # waist
+    40, 40, 40, 40,  40, 40, 40,  # arms
+    40, 40, 40, 40,  40, 40, 40   # arms
+]
+
+Kd = [
+    1, 1, 1, 2, 1, 1,     # legs
+    1, 1, 1, 2, 1, 1,     # legs
+    1, 1, 1,              # waist
+    1, 1, 1, 1, 1, 1, 1,  # arms
+    1, 1, 1, 1, 1, 1, 1   # arms 
+]
+
+class G1JointIndex:
+    LeftHipPitch = 0
+    LeftHipRoll = 1
+    LeftHipYaw = 2
+    LeftKnee = 3
+    LeftAnklePitch = 4
+    LeftAnkleB = 4
+    LeftAnkleRoll = 5
+    LeftAnkleA = 5
+    RightHipPitch = 6
+    RightHipRoll = 7
+    RightHipYaw = 8
+    RightKnee = 9
+    RightAnklePitch = 10
+    RightAnkleB = 10
+    RightAnkleRoll = 11
+    RightAnkleA = 11
+    WaistYaw = 12
+    WaistRoll = 13        # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistA = 13           # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistPitch = 14       # NOTE: INVALID for g1 23dof/29dof with waist locked
+    WaistB = 14           # NOTE: INVALID for g1 23dof/29dof with waist locked
+    LeftShoulderPitch = 15
+    LeftShoulderRoll = 16
+    LeftShoulderYaw = 17
+    LeftElbow = 18
+    LeftWristRoll = 19
+    LeftWristPitch = 20   # NOTE: INVALID for g1 23dof
+    LeftWristYaw = 21     # NOTE: INVALID for g1 23dof
+    RightShoulderPitch = 22
+    RightShoulderRoll = 23
+    RightShoulderYaw = 24
+    RightElbow = 25
+    RightWristRoll = 26
+    RightWristPitch = 27  # NOTE: INVALID for g1 23dof
+    RightWristYaw = 28    # NOTE: INVALID for g1 23dof
+
+
+class Mode:
+    PR = 0  # Series Control for Pitch/Roll Joints
+    AB = 1  # Parallel Control for A/B Joints
+
+class Custom:
+    def __init__(self):
+        self.time_ = 0.0
+        self.control_dt_ = 0.002  # [2ms]
+        self.duration_ = 3.0    # [3 s]
+        self.counter_ = 0
+        self.mode_pr_ = Mode.PR
+        self.mode_machine_ = 0
+        self.low_cmd = unitree_hg_msg_dds__LowCmd_()  
+        self.low_state = None 
+        self.update_mode_machine_ = False
+        self.crc = CRC()
+
+    def Init(self):
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+        # create publisher #
+        self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher_.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateHandler, 10)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=self.control_dt_, target=self.LowCmdWrite, name="control"
+        )
+        while self.update_mode_machine_ == False:
+            time.sleep(1)
+
+        if self.update_mode_machine_ == True:
+            self.lowCmdWriteThreadPtr.Start()
+
+    def LowStateHandler(self, msg: LowState_):
+        self.low_state = msg
+
+        if self.update_mode_machine_ == False:
+            self.mode_machine_ = self.low_state.mode_machine
+            self.update_mode_machine_ = True
+        
+        self.counter_ +=1
+        if (self.counter_ % 500 == 0) :
+            self.counter_ = 0
+            print(self.low_state.imu_state.rpy)
+
+    def LowCmdWrite(self):
+        self.time_ += self.control_dt_
+
+        if self.time_ < self.duration_ :
+            # [Stage 1]: set robot to zero posture
+            for i in range(G1_NUM_MOTOR):
+                ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
+                self.low_cmd.mode_pr = Mode.PR
+                self.low_cmd.mode_machine = self.mode_machine_
+                self.low_cmd.motor_cmd[i].mode =  1 # 1:Enable, 0:Disable
+                self.low_cmd.motor_cmd[i].tau = 0. 
+                self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q 
+                self.low_cmd.motor_cmd[i].dq = 0. 
+                self.low_cmd.motor_cmd[i].kp = Kp[i] 
+                self.low_cmd.motor_cmd[i].kd = Kd[i]
+
+        elif self.time_ < self.duration_ * 2 :
+            # [Stage 2]: swing ankle using PR mode
+            max_P = np.pi * 30.0 / 180.0
+            max_R = np.pi * 10.0 / 180.0
+            t = self.time_ - self.duration_
+            L_P_des = max_P * np.sin(2.0 * np.pi * t)
+            L_R_des = max_R * np.sin(2.0 * np.pi * t)
+            R_P_des = max_P * np.sin(2.0 * np.pi * t)
+            R_R_des = -max_R * np.sin(2.0 * np.pi * t)
+
+            self.low_cmd.mode_pr = Mode.PR
+            self.low_cmd.mode_machine = self.mode_machine_
+            self.low_cmd.motor_cmd[G1JointIndex.LeftAnklePitch].q = L_P_des
+            self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleRoll].q = L_R_des
+            self.low_cmd.motor_cmd[G1JointIndex.RightAnklePitch].q = R_P_des
+            self.low_cmd.motor_cmd[G1JointIndex.RightAnkleRoll].q = R_R_des
+
+        else :
+            # [Stage 3]: swing ankle using AB mode
+            max_A = np.pi * 30.0 / 180.0
+            max_B = np.pi * 10.0 / 180.0
+            t = self.time_ - self.duration_ * 2
+            L_A_des = max_A * np.sin(2.0 * np.pi * t)
+            L_B_des = max_B * np.sin(2.0 * np.pi * t + np.pi)
+            R_A_des = -max_A * np.sin(2.0 * np.pi * t)
+            R_B_des = -max_B * np.sin(2.0 * np.pi * t + np.pi)
+
+            self.low_cmd.mode_pr = Mode.AB
+            self.low_cmd.mode_machine = self.mode_machine_
+            self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleA].q = L_A_des
+            self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleB].q = L_B_des
+            self.low_cmd.motor_cmd[G1JointIndex.RightAnkleA].q = R_A_des
+            self.low_cmd.motor_cmd[G1JointIndex.RightAnkleB].q = R_B_des
+            
+            max_WristYaw = np.pi * 30.0 / 180.0
+            L_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t)
+            R_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t)
+            self.low_cmd.motor_cmd[G1JointIndex.LeftWristRoll].q = L_WristYaw_des
+            self.low_cmd.motor_cmd[G1JointIndex.RightWristRoll].q = R_WristYaw_des
+    
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher_.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:        
+        time.sleep(1)

example/go2/high_level/go2_sport_client.py

@@ -0,0 +1,170 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
+from unitree_sdk2py.go2.sport.sport_client import (
+    SportClient,
+    PathPoint,
+    SPORT_PATH_POINT_SIZE,
+)
+import math
+from dataclasses import dataclass
+
+@dataclass
+class TestOption:
+    name: str
+    id: int
+
+option_list = [
+    TestOption(name="damp", id=0),         
+    TestOption(name="stand_up", id=1),     
+    TestOption(name="stand_down", id=2),   
+    TestOption(name="move forward", id=3),         
+    TestOption(name="move lateral", id=4),    
+    TestOption(name="move rotate", id=5),  
+    TestOption(name="stop_move", id=6),  
+    TestOption(name="switch_gait", id=7),    
+    TestOption(name="switch_gait", id=8),
+    TestOption(name="balanced stand", id=9),     
+    TestOption(name="recovery", id=10),
+    TestOption(name="recovery", id=10),       
+    TestOption(name="left flip", id=11),      
+    TestOption(name="back flip", id=12),
+    TestOption(name="free walk", id=13),  
+    TestOption(name="free bound", id=14), 
+    TestOption(name="free avoid", id=15),  
+    TestOption(name="walk stair", id=16), 
+    TestOption(name="walk upright", id=17),
+    TestOption(name="cross step", id=18),
+    TestOption(name="free jump", id=19)       
+]
+
+class UserInterface:
+    def __init__(self):
+        self.test_option_ = None
+
+    def convert_to_int(self, input_str):
+        try:
+            return int(input_str)
+        except ValueError:
+            return None
+
+    def terminal_handle(self):
+        input_str = input("Enter id or name: \n")
+
+        if input_str == "list":
+            self.test_option_.name = None
+            self.test_option_.id = None
+            for option in option_list:
+                print(f"{option.name}, id: {option.id}")
+            return
+
+        for option in option_list:
+            if input_str == option.name or self.convert_to_int(input_str) == option.id:
+                self.test_option_.name = option.name
+                self.test_option_.id = option.id
+                print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
+                return
+
+        print("No matching test option found.")
+
+if __name__ == "__main__":
+
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} networkInterface")
+        sys.exit(-1)
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    test_option = TestOption(name=None, id=None) 
+    user_interface = UserInterface()
+    user_interface.test_option_ = test_option
+
+    sport_client = SportClient()  
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+    while True:
+
+        user_interface.terminal_handle()
+
+        print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n")
+
+        if test_option.id == 0:
+            sport_client.Damp()
+        elif test_option.id == 1:
+            sport_client.StandUp()
+        elif test_option.id == 2:
+            sport_client.StandDown()
+        elif test_option.id == 3:
+            sport_client.Move(0.3,0,0)
+        elif test_option.id == 4:
+            sport_client.Move(0,0.3,0)
+        elif test_option.id == 5:
+            sport_client.Move(0,0,0.5)
+        elif test_option.id == 6:
+            sport_client.StopMove()
+        elif test_option.id == 7:
+            sport_client.SwitchGait(0)
+        elif test_option.id == 8:
+            sport_client.SwitchGait(1)
+        elif test_option.id == 9:
+            sport_client.BalanceStand()
+        elif test_option.id == 10:
+            sport_client.RecoveryStand()
+        elif test_option.id == 11:
+            ret = sport_client.LeftFlip()
+            print("ret: ",ret)
+        elif test_option.id == 12:
+            ret = sport_client.BackFlip()
+            print("ret: ",ret)
+        elif test_option.id == 13:
+            ret = sport_client.FreeWalk(True)
+            print("ret: ",ret)
+        elif test_option.id == 14:
+            ret = sport_client.FreeBound(True)
+            print("ret: ",ret)
+            time.sleep(2)
+            ret = sport_client.FreeBound(False)
+            print("ret: ",ret)
+        elif test_option.id == 14:
+            ret = sport_client.FreeBound(True)
+            print("ret: ",ret)
+            time.sleep(2)
+            ret = sport_client.FreeBound(False)
+            print("ret: ",ret)
+        elif test_option.id == 15:
+            ret = sport_client.FreeAvoid(True)
+            print("ret: ",ret)
+            time.sleep(2)
+            ret = sport_client.FreeAvoid(False)
+            print("ret: ",ret)
+        elif test_option.id == 16:
+            ret = sport_client.WalkStair(True)
+            print("ret: ",ret)
+            time.sleep(10)
+            ret = sport_client.WalkStair(False)
+            print("ret: ",ret)
+        elif test_option.id == 17:
+            ret = sport_client.WalkUpright(True)
+            print("ret: ",ret)
+            time.sleep(4)
+            ret = sport_client.WalkUpright(False)
+            print("ret: ",ret)
+        elif test_option.id == 18:
+            ret = sport_client.CrossStep(True)
+            print("ret: ",ret)
+            time.sleep(4)
+            ret = sport_client.CrossStep(False)
+            print("ret: ",ret)
+        elif test_option.id == 19:
+            ret = sport_client.FreeJump(True)
+            print("ret: ",ret)
+            time.sleep(4)
+            ret = sport_client.FreeJump(False)
+            print("ret: ",ret)
+
+        time.sleep(1)

example/go2/high_level/go2_utlidar_switch.py

@@ -0,0 +1,39 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_
+from unitree_sdk2py.idl.default import std_msgs_msg_dds__String_
+
+class Custom:
+    def __init__(self):
+        # create publisher #
+        self.publisher = ChannelPublisher("rt/utlidar/switch", String_)
+        self.publisher.Init()
+        self.low_cmd = std_msgs_msg_dds__String_()   
+
+    def go2_utlidar_switch(self,status):
+        if status == "OFF":
+            self.low_cmd.data = "OFF"
+        elif status == "ON":
+            self.low_cmd.data = "ON"
+
+        self.publisher.Write(self.low_cmd)
+        
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.go2_utlidar_switch("OFF")
+
+
+

example/go2/low_level/go2_stand_example.py

@@ -0,0 +1,176 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+import unitree_legged_const as go2
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+from unitree_sdk2py.go2.sport.sport_client import SportClient
+
+class Custom:
+    def __init__(self):
+        self.Kp = 60.0
+        self.Kd = 5.0
+        self.time_consume = 0
+        self.rate_count = 0
+        self.sin_count = 0
+        self.motiontime = 0
+        self.dt = 0.002  # 0.001~0.01
+
+        self.low_cmd = unitree_go_msg_dds__LowCmd_()  
+        self.low_state = None  
+
+        self._targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                             -0.2, 1.36, -2.65, 0.2, 1.36, -2.65]
+        self._targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                             0.0, 0.67, -1.3, 0.0, 0.67, -1.3]
+        self._targetPos_3 = [-0.35, 1.36, -2.65, 0.35, 1.36, -2.65,
+                             -0.5, 1.36, -2.65, 0.5, 1.36, -2.65]
+
+        self.startPos = [0.0] * 12
+        self.duration_1 = 500
+        self.duration_2 = 500
+        self.duration_3 = 1000
+        self.duration_4 = 900
+        self.percent_1 = 0
+        self.percent_2 = 0
+        self.percent_3 = 0
+        self.percent_4 = 0
+
+        self.firstRun = True
+        self.done = False
+
+        # thread handling
+        self.lowCmdWriteThreadPtr = None
+
+        self.crc = CRC()
+
+    # Public methods
+    def Init(self):
+        self.InitLowCmd()
+
+        # create publisher #
+        self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
+
+        self.sc = SportClient()  
+        self.sc.SetTimeout(5.0)
+        self.sc.Init()
+
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.sc.StandDown()
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=0.002, target=self.LowCmdWrite, name="writebasiccmd"
+        )
+        self.lowCmdWriteThreadPtr.Start()
+
+    # Private methods
+    def InitLowCmd(self):
+        self.low_cmd.head[0]=0xFE
+        self.low_cmd.head[1]=0xEF
+        self.low_cmd.level_flag = 0xFF
+        self.low_cmd.gpio = 0
+        for i in range(20):
+            self.low_cmd.motor_cmd[i].mode = 0x01  # (PMSM) mode
+            self.low_cmd.motor_cmd[i].q= go2.PosStopF
+            self.low_cmd.motor_cmd[i].kp = 0
+            self.low_cmd.motor_cmd[i].dq = go2.VelStopF
+            self.low_cmd.motor_cmd[i].kd = 0
+            self.low_cmd.motor_cmd[i].tau = 0
+
+    def LowStateMessageHandler(self, msg: LowState_):
+        self.low_state = msg
+        # print("FR_0 motor state: ", msg.motor_state[go2.LegID["FR_0"]])
+        # print("IMU state: ", msg.imu_state)
+        # print("Battery state: voltage: ", msg.power_v, "current: ", msg.power_a)
+
+    def LowCmdWrite(self):
+
+        if self.firstRun:
+            for i in range(12):
+                self.startPos[i] = self.low_state.motor_state[i].q
+            self.firstRun = False
+
+        self.percent_1 += 1.0 / self.duration_1
+        self.percent_1 = min(self.percent_1, 1)
+        if self.percent_1 < 1:
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self._targetPos_1[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 <= 1):
+            self.percent_2 += 1.0 / self.duration_2
+            self.percent_2 = min(self.percent_2, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self._targetPos_1[i] + self.percent_2 * self._targetPos_2[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1):
+            self.percent_3 += 1.0 / self.duration_3
+            self.percent_3 = min(self.percent_3, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = self._targetPos_2[i] 
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1):
+            self.percent_4 += 1.0 / self.duration_4
+            self.percent_4 = min(self.percent_4, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self._targetPos_2[i] + self.percent_4 * self._targetPos_3[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:        
+        if custom.percent_4 == 1.0: 
+           time.sleep(1)
+           print("Done!")
+           sys.exit(-1)     
+        time.sleep(1)

example/go2/low_level/unitree_legged_const.py

@@ -0,0 +1,20 @@
+LegID = {
+    "FR_0": 0,  # Front right hip
+    "FR_1": 1,  # Front right thigh
+    "FR_2": 2,  # Front right calf
+    "FL_0": 3,
+    "FL_1": 4,
+    "FL_2": 5,
+    "RR_0": 6,
+    "RR_1": 7,
+    "RR_2": 8,
+    "RL_0": 9,
+    "RL_1": 10,
+    "RL_2": 11,
+}
+
+HIGHLEVEL = 0xEE
+LOWLEVEL = 0xFF
+TRIGERLEVEL = 0xF0
+PosStopF = 2.146e9
+VelStopF = 16000.0

example/go2w/high_level/go2w_sport_client.py

@@ -0,0 +1,99 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
+from unitree_sdk2py.go2.sport.sport_client import SportClient
+import math
+from dataclasses import dataclass
+
+@dataclass
+class TestOption:
+    name: str
+    id: int
+
+option_list = [
+    TestOption(name="damp", id=0),         
+    TestOption(name="stand_up", id=1),     
+    TestOption(name="stand_down", id=2),   
+    TestOption(name="move", id=3),         
+    TestOption(name="stop_move", id=4),    
+    TestOption(name="speed_level", id=5),  
+    TestOption(name="switch_gait", id=6),  
+    TestOption(name="get_state", id=7),    
+    TestOption(name="recovery", id=8),     
+    TestOption(name="balance", id=9)       
+]
+
+class UserInterface:
+    def __init__(self):
+        self.test_option_ = None
+
+    def convert_to_int(self, input_str):
+        try:
+            return int(input_str)
+        except ValueError:
+            return None
+
+    def terminal_handle(self):
+        input_str = input("Enter id or name: \n")
+
+        if input_str == "list":
+            self.test_option_.name = None
+            self.test_option_.id = None
+            for option in option_list:
+                print(f"{option.name}, id: {option.id}")
+            return
+
+        for option in option_list:
+            if input_str == option.name or self.convert_to_int(input_str) == option.id:
+                self.test_option_.name = option.name
+                self.test_option_.id = option.id
+                print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
+                return
+
+        print("No matching test option found.")
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} networkInterface")
+        sys.exit(-1)
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    test_option = TestOption(name=None, id=None) 
+    user_interface = UserInterface()
+    user_interface.test_option_ = test_option
+
+    sport_client = SportClient() 
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+
+    while True:
+        user_interface.terminal_handle()
+
+        print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n")
+
+        if test_option.id == 0:
+            sport_client.Damp()
+        elif test_option.id == 1:
+            sport_client.StandUp()
+        elif test_option.id == 2:
+            sport_client.StandDown()
+        elif test_option.id == 3:
+            sport_client.Move(0.5,0,0)
+        elif test_option.id == 4:
+            sport_client.StopMove()
+        elif test_option.id == 5:
+            sport_client.SpeedLevel(1)
+        elif test_option.id == 6:
+            sport_client.SwitchGait(1)
+        elif test_option.id == 8:
+            sport_client.RecoveryStand()
+        elif test_option.id == 9:
+            sport_client.BalanceStand()
+
+        time.sleep(1)

example/go2w/low_level/go2w_stand_example.py

@@ -0,0 +1,196 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+import unitree_legged_const as go2w
+from unitree_sdk2py.go2.robot_state.robot_state_client import RobotStateClient
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+from unitree_sdk2py.go2.sport.sport_client import SportClient
+
+class Custom:
+    def __init__(self):
+        self.Kp = 70.0
+        self.Kd = 5.0
+        self.time_consume = 0
+        self.rate_count = 0
+        self.sin_count = 0
+        self.motiontime = 0
+        self.dt = 0.002  
+
+        self.low_cmd = unitree_go_msg_dds__LowCmd_()  
+        self.low_state = None  
+
+        self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                             -0.2, 1.36, -2.65, 0.2, 1.36, -2.65]
+
+        self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                             0.0, 0.67, -1.3, 0.0, 0.67, -1.3]
+
+        self.targetPos_3 = [-0.35, 1.36, -2.65, 0.35, 1.36, -2.65,
+                             -0.5, 1.36, -2.65, 0.5, 1.36, -2.65]
+
+        self.startPos = [0.0] * 12
+        self.duration_1 = 500
+        self.duration_2 = 500
+        self.duration_3 = 2000
+        self.duration_4 = 900
+        self.percent_1 = 0
+        self.percent_2 = 0
+        self.percent_3 = 0
+        self.percent_4 = 0
+
+        self.firstRun = True
+        self.done = False
+
+        # thread handling
+        self.lowCmdWriteThreadPtr = None
+
+        self.crc = CRC()
+
+    # Public methods
+    def Init(self):
+        self.InitLowCmd()
+
+        # create publisher #
+        self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
+
+        self.sc = SportClient()  
+        self.sc.SetTimeout(5.0)
+        self.sc.Init()
+
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.sc.StandUp()
+            self.sc.StandDown()
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            name="writebasiccmd", interval=0.002, target=self.LowCmdWrite, 
+        )
+        self.lowCmdWriteThreadPtr.Start()
+
+    def InitLowCmd(self):
+        self.low_cmd.head[0] = 0xFE
+        self.low_cmd.head[1] = 0xEF
+        self.low_cmd.level_flag = 0xFF
+        self.low_cmd.gpio = 0
+        for i in range(20):
+            self.low_cmd.motor_cmd[i].mode = 0x01  
+            self.low_cmd.motor_cmd[i].q= go2w.PosStopF
+            self.low_cmd.motor_cmd[i].kp = 0
+            self.low_cmd.motor_cmd[i].dq = go2w.VelStopF
+            self.low_cmd.motor_cmd[i].kd = 0
+            self.low_cmd.motor_cmd[i].tau = 0
+
+    def LowStateMessageHandler(self, msg: LowState_):
+        self.low_state = msg
+
+    def LowCmdWrite(self):
+        if self.firstRun:
+            for i in range(12):
+                self.startPos[i] = self.low_state.motor_state[i].q
+            self.firstRun = False
+
+        self.percent_1 += 1.0 / self.duration_1
+        self.percent_1 = min(self.percent_1, 1)
+        if self.percent_1 < 1:
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 <= 1):
+            self.percent_2 += 1.0 / self.duration_2
+            self.percent_2 = min(self.percent_2, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1):
+            self.percent_3 += 1.0 / self.duration_3
+            self.percent_3 = min(self.percent_3, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = self.targetPos_2[i] 
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+            if self.percent_3 < 0.4:
+                for i in range(12, 16):
+                    self.low_cmd.motor_cmd[i].q = 0 
+                    self.low_cmd.motor_cmd[i].kp = 0.0
+                    self.low_cmd.motor_cmd[i].dq = 3
+                    self.low_cmd.motor_cmd[i].kd = self.Kd
+                    self.low_cmd.motor_cmd[i].tau = 0
+
+            if 0.4 <= self.percent_3 < 0.8:
+                for i in range(12, 16):
+                    self.low_cmd.motor_cmd[i].q = 0 
+                    self.low_cmd.motor_cmd[i].kp = 0
+                    self.low_cmd.motor_cmd[i].dq = -3
+                    self.low_cmd.motor_cmd[i].kd = self.Kd
+                    self.low_cmd.motor_cmd[i].tau = 0
+
+            if self.percent_3 >= 0.8:
+                for i in range(12, 16):
+                    self.low_cmd.motor_cmd[i].q = 0 
+                    self.low_cmd.motor_cmd[i].kp = 0
+                    self.low_cmd.motor_cmd[i].dq = 0
+                    self.low_cmd.motor_cmd[i].kd = self.Kd
+                    self.low_cmd.motor_cmd[i].tau = 0
+
+        if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1):
+            self.percent_4 += 1.0 / self.duration_4
+            self.percent_4 = min(self.percent_4, 1)
+            for i in range(12):
+                self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i]
+                self.low_cmd.motor_cmd[i].dq = 0
+                self.low_cmd.motor_cmd[i].kp = self.Kp
+                self.low_cmd.motor_cmd[i].kd = self.Kd
+                self.low_cmd.motor_cmd[i].tau = 0
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:   
+        if custom.percent_4 == 1.0: 
+           time.sleep(1)
+           print("Done!")
+           sys.exit(-1)     
+        time.sleep(1)

example/go2w/low_level/unitree_legged_const.py

@@ -0,0 +1,24 @@
+LegID = {
+    "FR_0": 0,  # Front right hip
+    "FR_1": 1,  # Front right thigh
+    "FR_2": 2,  # Front right calf
+    "FL_0": 3,
+    "FL_1": 4,
+    "FL_2": 5,
+    "RR_0": 6,
+    "RR_1": 7,
+    "RR_2": 8,
+    "RL_0": 9,
+    "RL_1": 10,
+    "RL_2": 11,
+    "FR_w": 12, # Front right wheel
+    "FL_w": 13, # Front left wheel
+    "RR_w": 14, # Rear right wheel
+    "RL_w": 15, # Rear left wheel
+}
+
+HIGHLEVEL = 0xEE
+LOWLEVEL = 0xFF
+TRIGERLEVEL = 0xF0
+PosStopF = 2.146e9
+VelStopF = 16000.0

example/h1/high_level/h1_loco_client_example.py

@@ -0,0 +1,96 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
+from unitree_sdk2py.h1.loco.h1_loco_client import LocoClient
+import math
+from dataclasses import dataclass
+
+@dataclass
+class TestOption:
+    name: str
+    id: int
+
+option_list = [
+    TestOption(name="damp", id=0),         
+    TestOption(name="stand_up", id=1),     
+    TestOption(name="move forward", id=3),         
+    TestOption(name="move lateral", id=4),    
+    TestOption(name="move rotate", id=5),  
+    TestOption(name="low stand", id=6),  
+    TestOption(name="high stand", id=7),    
+    TestOption(name="zero torque", id=8)     
+]
+
+class UserInterface:
+    def __init__(self):
+        self.test_option_ = None
+
+    def convert_to_int(self, input_str):
+        try:
+            return int(input_str)
+        except ValueError:
+            return None
+
+    def terminal_handle(self):
+        input_str = input("Enter id or name: \n")
+
+        if input_str == "list":
+            self.test_option_.name = None
+            self.test_option_.id = None
+            for option in option_list:
+                print(f"{option.name}, id: {option.id}")
+            return
+
+        for option in option_list:
+            if input_str == option.name or self.convert_to_int(input_str) == option.id:
+                self.test_option_.name = option.name
+                self.test_option_.id = option.id
+                print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
+                return
+
+        print("No matching test option found.")
+
+if __name__ == "__main__":
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    test_option = TestOption(name=None, id=None) 
+    user_interface = UserInterface()
+    user_interface.test_option_ = test_option
+
+    sport_client = LocoClient() 
+    sport_client.SetTimeout(10.0)
+    sport_client.Init()
+
+    while True:
+
+        user_interface.terminal_handle()
+
+        print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n")
+
+        if test_option.id == 0:
+            sport_client.Damp()
+        elif test_option.id == 1:
+            sport_client.StandUp()
+        elif test_option.id == 3:
+            sport_client.Move(0.3,0,0)
+        elif test_option.id == 4:
+            sport_client.Move(0,0.3,0)
+        elif test_option.id == 5:
+            sport_client.Move(0,0,0.3)
+        elif test_option.id == 6:
+            sport_client.LowStand()
+        elif test_option.id == 7:
+            sport_client.HighStand()
+        elif test_option.id == 8:
+            sport_client.ZeroTorque()
+
+        time.sleep(1)

example/h1/low_level/h1_low_level_example.py

@@ -0,0 +1,167 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+import unitree_legged_const as h1
+import numpy as np
+
+H1_NUM_MOTOR = 20
+
+class H1JointIndex:
+    # Right leg
+    kRightHipYaw = 8
+    kRightHipRoll = 0
+    kRightHipPitch = 1
+    kRightKnee = 2
+    kRightAnkle = 11
+    # Left leg
+    kLeftHipYaw = 7
+    kLeftHipRoll = 3
+    kLeftHipPitch = 4
+    kLeftKnee = 5
+    kLeftAnkle = 10
+
+    kWaistYaw = 6
+
+    kNotUsedJoint = 9
+
+    # Right arm
+    kRightShoulderPitch = 12
+    kRightShoulderRoll = 13
+    kRightShoulderYaw = 14
+    kRightElbow = 15
+    # Left arm
+    kLeftShoulderPitch = 16
+    kLeftShoulderRoll = 17
+    kLeftShoulderYaw = 18
+    kLeftElbow = 19
+
+class Custom:
+    def __init__(self):
+        self.time_ = 0.0
+        self.control_dt_ = 0.01  
+        self.duration_ = 10.0    
+        self.counter_ = 0
+        self.kp_low_ = 60.0
+        self.kp_high_ = 200.0
+        self.kd_low_ = 1.5
+        self.kd_high_ = 5.0
+        self.low_cmd = unitree_go_msg_dds__LowCmd_()  
+        self.InitLowCmd()
+        self.low_state = None 
+        self.crc = CRC()
+
+    def Init(self):
+        # # create publisher #
+        self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher_.Init()
+
+        # # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateHandler, 10)
+
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+        self.report_rpy_ptr_ = RecurrentThread(
+            interval=0.1, target=self.ReportRPY, name="report_rpy"
+        )
+
+        self.report_rpy_ptr_.Start()
+
+    def is_weak_motor(self,motor_index):
+        return motor_index in {
+            H1JointIndex.kLeftAnkle,
+            H1JointIndex.kRightAnkle,
+            H1JointIndex.kRightShoulderPitch,
+            H1JointIndex.kRightShoulderRoll,
+            H1JointIndex.kRightShoulderYaw,
+            H1JointIndex.kRightElbow,
+            H1JointIndex.kLeftShoulderPitch,
+            H1JointIndex.kLeftShoulderRoll,
+            H1JointIndex.kLeftShoulderYaw,
+            H1JointIndex.kLeftElbow,
+        }
+
+    def InitLowCmd(self):
+        self.low_cmd.head[0] = 0xFE
+        self.low_cmd.head[1] = 0xEF
+        self.low_cmd.level_flag = 0xFF
+        self.low_cmd.gpio = 0
+        for i in range(H1_NUM_MOTOR):
+            if self.is_weak_motor(i):
+                self.low_cmd.motor_cmd[i].mode = 0x01 
+            else:
+                self.low_cmd.motor_cmd[i].mode = 0x0A 
+            self.low_cmd.motor_cmd[i].q= h1.PosStopF
+            self.low_cmd.motor_cmd[i].kp = 0
+            self.low_cmd.motor_cmd[i].dq = h1.VelStopF
+            self.low_cmd.motor_cmd[i].kd = 0
+            self.low_cmd.motor_cmd[i].tau = 0
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=self.control_dt_, target=self.LowCmdWrite, name="control"
+        )
+        self.lowCmdWriteThreadPtr.Start()
+
+    def LowStateHandler(self, msg: LowState_):
+        self.low_state = msg
+
+    def ReportRPY(self):
+        print("rpy: [",self.low_state.imu_state.rpy[0],", "
+                    ,self.low_state.imu_state.rpy[1],", "
+                    ,self.low_state.imu_state.rpy[2],"]"
+        )
+
+    def LowCmdWrite(self):
+        self.time_ += self.control_dt_
+        self.time_ = np.clip(self.time_ , 0.0, self.duration_)
+
+        # set robot to zero posture
+        for i in range(H1_NUM_MOTOR):
+            ratio = self.time_ / self.duration_
+            self.low_cmd.motor_cmd[i].tau = 0. 
+            self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q 
+            self.low_cmd.motor_cmd[i].dq = 0. 
+            self.low_cmd.motor_cmd[i].kp = self.kp_low_ if self.is_weak_motor(i) else self.kp_high_
+            self.low_cmd.motor_cmd[i].kd = self.kd_low_ if self.is_weak_motor(i) else self.kd_high_
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher_.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:   
+        if custom.time_ == custom.duration_: 
+           time.sleep(1)
+           print("Done!")
+           sys.exit(-1)     
+        time.sleep(1)

example/h1/low_level/unitree_legged_const.py

@@ -0,0 +1,5 @@
+HIGHLEVEL = 0xEE
+LOWLEVEL = 0xFF
+TRIGERLEVEL = 0xF0
+PosStopF = 2.146e9
+VelStopF = 16000.0

example/h1_2/low_level/h1_2_low_level_example.py

@@ -0,0 +1,201 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+from unitree_sdk2py.utils.crc import CRC
+from unitree_sdk2py.utils.thread import RecurrentThread
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+import numpy as np
+
+H1_2_NUM_MOTOR = 27
+
+class H1_2_JointIndex:
+    # legs
+    LeftHipYaw = 0
+    LeftHipPitch = 1
+    LeftHipRoll = 2
+    LeftKnee = 3
+    LeftAnklePitch = 4
+    LeftAnkleB = 4
+    LeftAnkleRoll = 5
+    LeftAnkleA = 5
+    RightHipYaw = 6
+    RightHipPitch = 7
+    RightHipRoll = 8
+    RightKnee = 9
+    RightAnklePitch = 10
+    RightAnkleB = 10
+    RightAnkleRoll = 11
+    RightAnkleA = 11
+    # torso
+    WaistYaw = 12
+    # arms
+    LeftShoulderPitch = 13
+    LeftShoulderRoll = 14
+    LeftShoulderYaw = 15
+    LeftElbow = 16
+    LeftWristRoll = 17
+    LeftWristPitch = 18
+    LeftWristYaw = 19
+    RightShoulderPitch = 20
+    RightShoulderRoll = 21
+    RightShoulderYaw = 22
+    RightElbow = 23
+    RightWristRoll = 24
+    RightWristPitch = 25
+    RightWristYaw = 26
+
+
+class Mode:
+    PR = 0  # Series Control for Pitch/Roll Joints
+    AB = 1  # Parallel Control for A/B Joints
+
+class Custom:
+    def __init__(self):
+        self.time_ = 0.0
+        self.control_dt_ = 0.002  # [2ms]
+        self.duration_ = 3.0    # [3 s]
+        self.counter_ = 0
+        self.mode_pr_ = Mode.PR
+        self.mode_machine_ = 0
+        self.low_cmd = unitree_hg_msg_dds__LowCmd_()  
+        self.low_state = None 
+        self.update_mode_machine_ = False
+        self.crc = CRC()
+
+    def Init(self):
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+        status, result = self.msc.CheckMode()
+        while result['name']:
+            self.msc.ReleaseMode()
+            status, result = self.msc.CheckMode()
+            time.sleep(1)
+
+        # create publisher #
+        self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
+        self.lowcmd_publisher_.Init()
+
+        # create subscriber # 
+        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateHandler, 10)
+
+    def Start(self):
+        self.lowCmdWriteThreadPtr = RecurrentThread(
+            interval=self.control_dt_, target=self.LowCmdWrite, name="control"
+        )
+        while self.update_mode_machine_ == False:
+            time.sleep(1)
+
+        if self.update_mode_machine_ == True:
+            self.lowCmdWriteThreadPtr.Start()
+
+    def LowStateHandler(self, msg: LowState_):
+        self.low_state = msg
+
+        if self.update_mode_machine_ == False:
+            self.mode_machine_ = self.low_state.mode_machine
+            self.update_mode_machine_ = True
+        
+        self.counter_ +=1
+        if (self.counter_ % 500 == 0) :
+            self.counter_ = 0
+            print(self.low_state.imu_state.rpy)
+
+    def LowCmdWrite(self):
+        self.time_ += self.control_dt_
+        self.low_cmd.mode_pr = Mode.PR
+        self.low_cmd.mode_machine = self.mode_machine_
+        for i in range(H1_2_NUM_MOTOR):
+            ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
+            self.low_cmd.motor_cmd[i].mode =  1 # 1:Enable, 0:Disable
+            self.low_cmd.motor_cmd[i].tau = 0.0 
+            self.low_cmd.motor_cmd[i].q = 0.0
+            self.low_cmd.motor_cmd[i].dq = 0.0
+            self.low_cmd.motor_cmd[i].kp = 100.0 if i < 13 else 50.0
+            self.low_cmd.motor_cmd[i].kd = 1.0
+
+        if self.time_ < self.duration_ :
+            # [Stage 1]: set robot to zero posture
+            for i in range(H1_2_NUM_MOTOR):
+                ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
+                self.low_cmd.mode_pr = Mode.PR
+                self.low_cmd.mode_machine = self.mode_machine_
+                self.low_cmd.motor_cmd[i].mode =  1 # 1:Enable, 0:Disable
+                self.low_cmd.motor_cmd[i].tau = 0. 
+                self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q 
+                self.low_cmd.motor_cmd[i].dq = 0. 
+                self.low_cmd.motor_cmd[i].kp = 100.0 if i < 13 else 50.0
+                self.low_cmd.motor_cmd[i].kd = 1.0
+        else :
+            # [Stage 2]: swing ankle using PR mode
+            max_P = 0.25
+            max_R = 0.25
+            t = self.time_ - self.duration_ 
+            L_P_des = max_P * np.cos(2.0 * np.pi * t)
+            L_R_des = max_R * np.sin(2.0 * np.pi * t)
+            R_P_des = max_P * np.cos(2.0 * np.pi * t)
+            R_R_des = -max_R * np.sin(2.0 * np.pi * t)
+
+            Kp_Pitch = 80
+            Kd_Pitch = 1
+            Kp_Roll = 80
+            Kd_Roll = 1
+
+            self.low_cmd.mode_pr = Mode.PR
+            self.low_cmd.mode_machine = self.mode_machine_
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].q = L_P_des
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].dq = 0
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].kp = Kp_Pitch
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnklePitch].kd = Kd_Pitch
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].q = L_R_des
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].dq = 0
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].kp = Kp_Roll
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftAnkleRoll].kd = Kd_Roll
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].q = R_P_des
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].dq = 0
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].kp = Kp_Pitch
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnklePitch].kd = Kd_Pitch
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].q = R_R_des
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].dq = 0
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].kp = Kp_Roll
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightAnkleRoll].kd = Kd_Roll
+
+            max_wrist_roll_angle = 0.5;  # [rad]
+            WristRoll_des = max_wrist_roll_angle * np.sin(2.0 * np.pi * t)
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].q = WristRoll_des
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].dq = 0
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].kp = 50
+            self.low_cmd.motor_cmd[H1_2_JointIndex.LeftWristRoll].kd = 1
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].q = WristRoll_des
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].dq = 0
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].kp = 50
+            self.low_cmd.motor_cmd[H1_2_JointIndex.RightWristRoll].kd = 1
+
+        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
+        self.lowcmd_publisher_.Write(self.low_cmd)
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    custom.Init()
+    custom.Start()
+
+    while True:        
+        time.sleep(1)

example/hg/arm_sdk.py

@@ -1,133 +0,0 @@
-import time
-import numpy as np
-from enum import IntEnum
-from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
-
-# from user_data import *
-from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
-from unitree_sdk2py.idl.unitree_hg.msg.dds_ import MotorCmd_
-from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
-import unitree_sdk2py.idl.unitree_hg.msg.dds_ as dds
-print(dds.LowCmd_)
-print(dds.MotorCmd_)
-
-kTopicArmSDK = "rt/arm_sdk"
-kPi = 3.141592654
-kPi_2 = 1.57079632
-
-
-class JointIndex(IntEnum):
-
-    #   // Left arm
-    kLeftShoulderPitch = 13
-    kLeftShoulderRoll = 14
-    kLeftShoulderYaw = 15
-    kLeftElbow = 16
-    kLeftWistYaw = 17
-    kLeftWistPitch = 18
-    kLeftWistRoll = 19
-    #   // Right arm
-    kRightShoulderPitch = 20
-    kRightShoulderRoll = 21
-    kRightShoulderYaw = 22
-    kRightElbow = 23
-    kRightWistYaw = 24
-    kRightWistPitch = 25
-    kRightWistRoll = 26
-
-    kWaistYaw = 12
-
-
-kNotUsedJoint = 27
-
-
-if __name__ == "__main__":
-    ChannelFactoryInitialize()
-
-    # Create a publisher to publish the data defined in UserData class
-    arm_sdk_publisher = ChannelPublisher('rt/arm_sdk', LowCmd_)
-    # pub = ChannelPublisher("rt/lowcmd", LowCmd_)
-    arm_sdk_publisher.Init()
-
-    msg = unitree_hg_msg_dds__LowCmd_()
-
-    weight = 0
-    weight_rate = 0.2
-
-    kp = 60
-    kd = 1.5
-    dq = 0
-    tau_ff = 0
-
-    control_dt = 0.02
-    max_joint_velocity = 0.5
-
-    delta_weight = weight_rate * control_dt
-    max_joint_delta = max_joint_velocity * control_dt
-
-    init_pos = np.zeros(15)
-    target_pos = np.array(
-        [
-            0.0,
-            kPi_2,
-            0.0,
-            kPi_2,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-            -kPi_2,
-            0.0,
-            kPi_2,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-        ]
-    )
-    print("Initailizing arms ...")
-
-    init_time = 5
-    init_time_steps = int(init_time / control_dt)
-
-    for i in range(init_time_steps):
-        weight += delta_weight
-        weight = max(min(weight, 1.0), 0)
-        msg.motor_cmd[kNotUsedJoint].q = weight * weight
-
-        for idx, joint in enumerate(JointIndex):
-            msg.motor_cmd[joint].q = init_pos[idx]
-            msg.motor_cmd[joint].dq = dq
-            msg.motor_cmd[joint].kp = kp
-            msg.motor_cmd[joint].kd = kd
-            msg.motor_cmd[joint].tau = tau_ff
-
-        arm_sdk_publisher.Write(msg)
-
-        time.sleep(control_dt)
-
-    print("Done!")
-
-    period = 5
-    num_time_steps = int(period / control_dt)
-
-    current_jpos_des = np.zeros_like(init_pos)
-
-    for i in range(num_time_steps):
-        for j in range(len(current_jpos_des)):
-                delta = target_pos[j] - current_jpos_des[j]
-                clamped_delta = np.clip(delta, -max_joint_delta, max_joint_delta)
-                current_jpos_des[j] += clamped_delta
-        
-        for idx, joint in enumerate(JointIndex):
-            msg.motor_cmd[joint].q = current_jpos_des[idx]
-            msg.motor_cmd[joint].dq = dq
-            msg.motor_cmd[joint].kp = kp
-            msg.motor_cmd[joint].kd = kd
-            msg.motor_cmd[joint].tau = tau_ff
-        
-        arm_sdk_publisher.Write(msg)
-        
-        time.sleep(control_dt)
-    
-    exit()

example/hg/low_cmd.py

@@ -1,50 +0,0 @@
-import time
-import numpy as np
-from enum import IntEnum
-from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
-import sys
-
-# from user_data import *
-from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
-from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
-import unitree_sdk2py.idl.unitree_hg.msg.dds_ as dds
-from unitree_sdk2py.utils.crc import CRC
-
-
-kTopicArmSDK = "rt/lowcmd"
-kPi = 3.141592654
-kPi_2 = 1.57079632
-kNumMotors = 35
-
-if __name__ == "__main__":
-
-    if len(sys.argv)>1:
-        ChannelFactoryInitialize(0, sys.argv[1])
-    else:
-        ChannelFactoryInitialize(0)
-    
-    # 初始化发布节点
-    low_cmd_publisher = ChannelPublisher(kTopicArmSDK, LowCmd_)
-    low_cmd_publisher.Init()
-    # 初始化LowCmd_
-    msg = unitree_hg_msg_dds__LowCmd_()
-    # CRC校验实例
-    crc = CRC()
-    # 控制时间参数
-    control_dt = 0.02
-    init_time = 20
-    init_time_steps = int(init_time / control_dt)
-
-    # 控制参数设置
-    msg.mode_pr = 0
-    msg.mode_machine = 4
-    for i in range(init_time_steps):
-        for idx in range(kNumMotors):
-                msg.motor_cmd[idx].mode = 0x01
-        msg.crc = crc.Crc(msg)
-        # print(msg.crc)
-        low_cmd_publisher.Write(msg)
-        time.sleep(control_dt)
-
-    print("Done!")
-    exit()

example/high_level/read_highstate.py

@@ -1,24 +0,0 @@
-import time
-import sys
-from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
-
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
-
-
-def HighStateHandler(msg: SportModeState_):
-    print("Position: ", msg.position)
-    print("Velocity: ", msg.velocity)
-    print("Yaw velocity: ", msg.yaw_speed)
-    print("Foot position in body frame: ", msg.foot_position_body)
-    print("Foot velocity in body frame: ", msg.foot_speed_body)
-
-
-if __name__ == "__main__":
-    # sys.argv[1]: name of the network interface
-    ChannelFactoryInitialize(0, sys.argv[1])
-    sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)
-    sub.Init(HighStateHandler, 10)
-
-    while True:
-        time.sleep(10.0)

example/high_level/sportmode_test.py

@@ -1,144 +0,0 @@
-import time
-import sys
-from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
-from unitree_sdk2py.go2.sport.sport_client import (
-    SportClient,
-    PathPoint,
-    SPORT_PATH_POINT_SIZE,
-)
-import math
-
-
-class SportModeTest:
-    def __init__(self) -> None:
-        # Time count
-        self.t = 0
-        self.dt = 0.01
-
-        # Initial poition and yaw
-        self.px0 = 0
-        self.py0 = 0
-        self.yaw0 = 0
-
-        self.client = SportClient()  # Create a sport client
-        self.client.SetTimeout(10.0)
-        self.client.Init()
-
-    def GetInitState(self, robot_state: SportModeState_):
-        self.px0 = robot_state.position[0]
-        self.py0 = robot_state.position[1]
-        self.yaw0 = robot_state.imu_state.rpy[2]
-
-    def StandUpDown(self):
-        self.client.StandDown()
-        print("Stand down !!!")
-        time.sleep(1)
-
-        self.client.StandUp()
-        print("Stand up !!!")
-        time.sleep(1)
-
-        self.client.StandDown()
-        print("Stand down !!!")
-        time.sleep(1)
-
-        self.client.Damp()
-
-    def VelocityMove(self):
-        elapsed_time = 1
-        for i in range(int(elapsed_time / self.dt)):
-            self.client.Move(0.3, 0, 0.3)  # vx, vy vyaw
-            time.sleep(self.dt)
-        self.client.StopMove()
-
-    def BalanceAttitude(self):
-        self.client.Euler(0.1, 0.2, 0.3)  # roll, pitch, yaw
-        self.client.BalanceStand()
-
-    def TrajectoryFollow(self):
-        time_seg = 0.2
-        time_temp = self.t - time_seg
-        path = []
-        for i in range(SPORT_PATH_POINT_SIZE):
-            time_temp += time_seg
-
-            px_local = 0.5 * math.sin(0.5 * time_temp)
-            py_local = 0
-            yaw_local = 0
-            vx_local = 0.25 * math.cos(0.5 * time_temp)
-            vy_local = 0
-            vyaw_local = 0
-
-            path_point_tmp = PathPoint(0, 0, 0, 0, 0, 0, 0)
-
-            path_point_tmp.timeFromStart = i * time_seg
-            path_point_tmp.x = (
-                px_local * math.cos(self.yaw0)
-                - py_local * math.sin(self.yaw0)
-                + self.px0
-            )
-            path_point_tmp.y = (
-                px_local * math.sin(self.yaw0)
-                + py_local * math.cos(self.yaw0)
-                + self.py0
-            )
-            path_point_tmp.yaw = yaw_local + self.yaw0
-            path_point_tmp.vx = vx_local * math.cos(self.yaw0) - vy_local * math.sin(
-                self.yaw0
-            )
-            path_point_tmp.vy = vx_local * math.sin(self.yaw0) + vy_local * math.cos(
-                self.yaw0
-            )
-            path_point_tmp.vyaw = vyaw_local
-
-            path.append(path_point_tmp)
-
-            self.client.TrajectoryFollow(path)
-            
-    def SpecialMotions(self):
-        self.client.RecoveryStand()
-        print("RecoveryStand !!!")
-        time.sleep(1)
-        
-        self.client.Stretch()
-        print("Sit !!!")
-        time.sleep(1)  
-        
-        self.client.RecoveryStand()
-        print("RecoveryStand !!!")
-        time.sleep(1)
-
-
-# Robot state
-robot_state = unitree_go_msg_dds__SportModeState_()
-def HighStateHandler(msg: SportModeState_):
-    global robot_state
-    robot_state = msg
-
-
-if __name__ == "__main__":
-    if len(sys.argv)>1:
-        ChannelFactoryInitialize(0, sys.argv[1])
-    else:
-        ChannelFactoryInitialize(0)
-        
-    sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)
-    sub.Init(HighStateHandler, 10)
-    time.sleep(1)
-
-    test = SportModeTest()
-    test.GetInitState(robot_state)
-
-    print("Start test !!!")
-    while True:
-        test.t += test.dt
-
-        test.StandUpDown()
-        # test.VelocityMove()
-        # test.BalanceAttitude()
-        # test.TrajectoryFollow()
-        # test.SpecialMotions()
-
-        time.sleep(test.dt)

example/low_level/lowlevel_control.py

@@ -1,60 +0,0 @@
-import time
-import sys
-
-from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
-from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
-from unitree_sdk2py.utils.crc import CRC
-from unitree_sdk2py.utils.thread import Thread
-import unitree_legged_const as go2
-
-crc = CRC()
-
-if __name__ == '__main__':
-
-    if len(sys.argv)>1:
-        ChannelFactoryInitialize(0, sys.argv[1])
-    else:
-        ChannelFactoryInitialize(0)
-    # Create a publisher to publish the data defined in UserData class
-    pub = ChannelPublisher("rt/lowcmd", LowCmd_)
-    pub.Init()
-    
-    cmd = unitree_go_msg_dds__LowCmd_()
-    cmd.head[0]=0xFE
-    cmd.head[1]=0xEF
-    cmd.level_flag = 0xFF
-    cmd.gpio = 0
-    for i in range(20):
-        cmd.motor_cmd[i].mode = 0x01  # (PMSM) mode
-        cmd.motor_cmd[i].q= go2.PosStopF
-        cmd.motor_cmd[i].kp = 0
-        cmd.motor_cmd[i].dq = go2.VelStopF
-        cmd.motor_cmd[i].kd = 0
-        cmd.motor_cmd[i].tau = 0
-
-    while True:        
-        # Toque controle, set RL_2 toque
-        cmd.motor_cmd[go2.LegID["RL_2"]].q = 0.0 # Set to stop position(rad)
-        cmd.motor_cmd[go2.LegID["RL_2"]].kp = 0.0
-        cmd.motor_cmd[go2.LegID["RL_2"]].dq = 0.0 # Set to stop angular velocity(rad/s)
-        cmd.motor_cmd[go2.LegID["RL_2"]].kd = 0.0
-        cmd.motor_cmd[go2.LegID["RL_2"]].tau = 1.0 # target toque is set to 1N.m
-
-        # Poinstion(rad) control, set RL_0 rad
-        cmd.motor_cmd[go2.LegID["RL_0"]].q = 0.0  # Taregt angular(rad)
-        cmd.motor_cmd[go2.LegID["RL_0"]].kp = 10.0 # Poinstion(rad) control kp gain
-        cmd.motor_cmd[go2.LegID["RL_0"]].dq = 0.0  # Taregt angular velocity(rad/ss)
-        cmd.motor_cmd[go2.LegID["RL_0"]].kd = 1.0  # Poinstion(rad) control kd gain
-        cmd.motor_cmd[go2.LegID["RL_0"]].tau = 0.0 # Feedforward toque 1N.m
-        
-        cmd.crc = crc.Crc(cmd)
-
-        #Publish message
-        if pub.Write(cmd):
-            print("Publish success. msg:", cmd.crc)
-        else:
-            print("Waitting for subscriber.")
-
-        time.sleep(0.002)

example/low_level/read_lowstate.py

@@ -1,27 +0,0 @@
-import time
-import sys
-from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
-
-import unitree_legged_const as go2
-
-
-def LowStateHandler(msg: LowState_):
-    
-    # print front right hip motor states
-    print("FR_0 motor state: ", msg.motor_state[go2.LegID["FR_0"]])
-    print("IMU state: ", msg.imu_state)
-    print("Battery state: voltage: ", msg.power_v, "current: ", msg.power_a)
-
-
-if __name__ == "__main__":
-    if len(sys.argv)>1:
-        ChannelFactoryInitialize(0, sys.argv[1])
-    else:
-        ChannelFactoryInitialize(0)
-    sub = ChannelSubscriber("rt/lowstate", LowState_)
-    sub.Init(LowStateHandler, 10)
-
-    while True:
-        time.sleep(10.0)

example/motionSwitcher/motion_switcher_example.py

@@ -0,0 +1,36 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+
+class Custom:
+    def __init__(self):
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+
+    def selectMode(self,name):
+        ret = self.msc.SelectMode(name)
+        return ret
+
+
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
+    if len(sys.argv)>1:
+        ChannelFactoryInitialize(0, sys.argv[1])
+    else:
+        ChannelFactoryInitialize(0)
+
+    custom = Custom()
+    selectMode = "ai" 
+    # selectMode = "normal"
+    # selectMode = "advanced" 
+    # selectMode = "ai-w"  # for wheeled robot
+    ret = custom.selectMode(selectMode) 
+    print("ret: ",ret)
+

example/test_crc.py

@@ -1,46 +0,0 @@
-import time
-import sys
-
-from unitree_sdk2py.core.channel import ChannelFactoryInitialize
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
-from unitree_sdk2py.utils.crc import CRC
-
-crc = CRC()
-
-PosStopF = 2.146e9
-VelStopF = 16000.0
-
-if __name__ == "__main__":
-    ChannelFactoryInitialize(0)
-
-    cmd = unitree_go_msg_dds__LowCmd_()
-    cmd.head[0] = 0xFE
-    cmd.head[1] = 0xEF
-    cmd.level_flag = 0xFF
-    cmd.gpio = 0
-    for i in range(20):
-        cmd.motor_cmd[i].mode = 0x01
-        cmd.motor_cmd[i].q = PosStopF
-        cmd.motor_cmd[i].kp = 0
-        cmd.motor_cmd[i].dq = VelStopF
-        cmd.motor_cmd[i].kd = 0
-        cmd.motor_cmd[i].tau = 0
-
-    cmd.motor_cmd[0].q = 0.0
-    cmd.motor_cmd[0].kp = 0.0
-    cmd.motor_cmd[0].dq = 0.0
-    cmd.motor_cmd[0].kd = 0.0
-    cmd.motor_cmd[0].tau = 1.0
-
-    cmd.motor_cmd[1].q = 0.0
-    cmd.motor_cmd[1].kp = 10.0
-    cmd.motor_cmd[1].dq = 0.0
-    cmd.motor_cmd[1].kd = 1.0
-    cmd.motor_cmd[1].tau = 0.0
-
-    now = time.perf_counter()
-    
-
-    cmd.crc = crc.Crc(cmd)
-
-    print("CRC:", cmd.crc, "Time cost:", (time.perf_counter() - now)*1000)

example/wireless_controller/wireless_controller.py

@@ -1,55 +1,131 @@
 import time
 import sys
+import struct
+
 from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
 
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__WirelessController_
+# Uncomment the following two lines when using Go2、Go2-W、B2、B2-W、H1 robot
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
+# from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+# from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+
+# Uncomment the following two lines when using G1、H1-2 robot
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+
+class unitreeRemoteController:
+    def __init__(self):
+        # key
+        self.Lx = 0           
+        self.Rx = 0            
+        self.Ry = 0            
+        self.Ly = 0
+
+        # button
+        self.L1 = 0
+        self.L2 = 0
+        self.R1 = 0
+        self.R2 = 0
+        self.A = 0
+        self.B = 0
+        self.X = 0
+        self.Y = 0
+        self.Up = 0
+        self.Down = 0
+        self.Left = 0
+        self.Right = 0
+        self.Select = 0
+        self.F1 = 0
+        self.F3 = 0
+        self.Start = 0
+       
+    def parse_botton(self,data1,data2):
+        self.R1 = (data1 >> 0) & 1
+        self.L1 = (data1 >> 1) & 1
+        self.Start = (data1 >> 2) & 1
+        self.Select = (data1 >> 3) & 1
+        self.R2 = (data1 >> 4) & 1
+        self.L2 = (data1 >> 5) & 1
+        self.F1 = (data1 >> 6) & 1
+        self.F3 = (data1 >> 7) & 1
+        self.A = (data2 >> 0) & 1
+        self.B = (data2 >> 1) & 1
+        self.X = (data2 >> 2) & 1
+        self.Y = (data2 >> 3) & 1
+        self.Up = (data2 >> 4) & 1
+        self.Right = (data2 >> 5) & 1
+        self.Down = (data2 >> 6) & 1
+        self.Left = (data2 >> 7) & 1
+
+    def parse_key(self,data):
+        lx_offset = 4
+        self.Lx = struct.unpack('<f', data[lx_offset:lx_offset + 4])[0]
+        rx_offset = 8
+        self.Rx = struct.unpack('<f', data[rx_offset:rx_offset + 4])[0]
+        ry_offset = 12
+        self.Ry = struct.unpack('<f', data[ry_offset:ry_offset + 4])[0]
+        L2_offset = 16
+        L2 = struct.unpack('<f', data[L2_offset:L2_offset + 4])[0] # Placeholder，unused
+        ly_offset = 20
+        self.Ly = struct.unpack('<f', data[ly_offset:ly_offset + 4])[0]
 
 
-key_state = [
+    def parse(self,remoteData):
-    ["R1", 0],
+        self.parse_key(remoteData)
-    ["L1", 0],
+        self.parse_botton(remoteData[2],remoteData[3])
-    ["start", 0],
+
-    ["select", 0],
+        print("debug unitreeRemoteController: ")
-    ["R2", 0],
+        print("Lx:", self.Lx)
-    ["L2", 0],
+        print("Rx:", self.Rx)
-    ["F1", 0],
+        print("Ry:", self.Ry)
-    ["F2", 0],
+        print("Ly:", self.Ly)
-    ["A", 0],
+
-    ["B", 0],
+        print("L1:", self.L1)
-    ["X", 0],
+        print("L2:", self.L2)
-    ["Y", 0],
+        print("R1:", self.R1)
-    ["up", 0],
+        print("R2:", self.R2)
-    ["right", 0],
+        print("A:", self.A)
-    ["down", 0],
+        print("B:", self.B)
-    ["left", 0],
+        print("X:", self.X)
-]
+        print("Y:", self.Y)
+        print("Up:", self.Up)
+        print("Down:", self.Down)
+        print("Left:", self.Left)
+        print("Right:", self.Right)
+        print("Select:", self.Select)
+        print("F1:", self.F1)
+        print("F3:", self.F3)
+        print("Start:", self.Start)
+        print("\n")
+
+        
+class Custom:
+    def __init__(self):
+        self.low_state = None 
+        self.remoteController = unitreeRemoteController()
+
+    def Init(self):
+        self.lowstate_subscriber = ChannelSubscriber("rt/lf/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
+
+    
+    def LowStateMessageHandler(self, msg: LowState_):
+        self.low_state = msg
+        wireless_remote_data = self.low_state.wireless_remote
+        self.remoteController.parse(wireless_remote_data)
 
 
-def WirelessControllerHandler(msg: WirelessController_):
+if __name__ == '__main__':
-    global key_state
-    print("lx: ", msg.lx)
-    print("lx: ", msg.ly)
-    print("lx: ", msg.rx)
-    print("lx: ", msg.ry)
-    print("keys: ", msg.keys)
 
-    #Update key state
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
-    for i in range(16):
+    input("Press Enter to continue...")
-        key_state[i][1] = (msg.keys & (1 << i)) >> i
 
-    print(key_state)
-
-
-
-if __name__ == "__main__":
     if len(sys.argv)>1:
         ChannelFactoryInitialize(0, sys.argv[1])
     else:
         ChannelFactoryInitialize(0)
-        
+
-    sub = ChannelSubscriber("rt/wirelesscontroller", WirelessController_)
+    custom = Custom()
-    sub.Init(WirelessControllerHandler, 10)
+    custom.Init()
-    
+
-    while True:
+    while True:   
-        time.sleep(10.0)
+        time.sleep(1)

pyproject.toml

@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools", "wheel"]
+build-backend = "setuptools.build_meta"

setup.py

@@ -1,16 +1,21 @@
 from setuptools import setup, find_packages
 
 setup(name='unitree_sdk2py',
-      version='1.0.0',
+      version='1.0.1',
-      author='Unitree',
+      author='UnitreeRobotics',
       author_email='unitree@unitree.com',
+      long_description=open('README.md').read(),
+      long_description_content_type="text/markdown",
       license="BSD-3-Clause",
       packages=find_packages(include=['unitree_sdk2py','unitree_sdk2py.*']),
       description='Unitree robot sdk version 2 for python',
+      project_urls={
+            "Source Code": "https://github.com/unitreerobotics/unitree_sdk2_python",
+      },
       python_requires='>=3.8',
       install_requires=[
             "cyclonedds==0.10.2",
             "numpy",
             "opencv-python",
       ],
-      )
+      )

unitree_sdk2py/b2/back_video/back_video_api.py

@@ -0,0 +1,16 @@
+"""
+" service name
+"""
+ROBOT_BACK_VIDEO_SERVICE_NAME = "back_videohub"
+
+
+"""
+" service api version
+"""
+ROBOT_BACK_VIDEO_API_VERSION = "1.0.0.0"
+
+
+"""
+" api id
+"""
+ROBOT_BACK_VIDEO_API_ID_GETIMAGESAMPLE = 1001

unitree_sdk2py/b2/back_video/back_video_client.py

@@ -0,0 +1,23 @@
+import json
+
+from ...rpc.client import Client
+from .back_video_api import *
+
+
+"""
+" class FrontVideoClient
+"""
+class BackVideoClient(Client):
+    def __init__(self):
+        super().__init__(ROBOT_BACK_VIDEO_SERVICE_NAME, False)
+
+
+    def Init(self):
+        # set api version
+        self._SetApiVerson(ROBOT_BACK_VIDEO_API_VERSION)
+        # regist api
+        self._RegistApi(ROBOT_BACK_VIDEO_API_ID_GETIMAGESAMPLE, 0)
+
+    # 1001
+    def GetImageSample(self):
+        return self._CallBinary(ROBOT_BACK_VIDEO_API_ID_GETIMAGESAMPLE, [])

unitree_sdk2py/b2/front_video/front_video_api.py

@@ -0,0 +1,16 @@
+"""
+" service name
+"""
+ROBOT_FRONT_VIDEO_SERVICE_NAME = "front_videohub"
+
+
+"""
+" service api version
+"""
+ROBOT_FRONT_VIDEO_API_VERSION = "1.0.0.0"
+
+
+"""
+" api id
+"""
+ROBOT_FRONT_VIDEO_API_ID_GETIMAGESAMPLE = 1001

unitree_sdk2py/b2/front_video/front_video_client.py

@@ -0,0 +1,23 @@
+import json
+
+from ...rpc.client import Client
+from .front_video_api import *
+
+
+"""
+" class FrontVideoClient
+"""
+class FrontVideoClient(Client):
+    def __init__(self):
+        super().__init__(ROBOT_FRONT_VIDEO_SERVICE_NAME, False)
+
+
+    def Init(self):
+        # set api version
+        self._SetApiVerson(ROBOT_FRONT_VIDEO_API_VERSION)
+        # regist api
+        self._RegistApi(ROBOT_FRONT_VIDEO_API_ID_GETIMAGESAMPLE, 0)
+
+    # 1001
+    def GetImageSample(self):
+        return self._CallBinary(ROBOT_FRONT_VIDEO_API_ID_GETIMAGESAMPLE, [])

unitree_sdk2py/b2/video/video_api.py

@@ -1,16 +0,0 @@
-"""
-" service name
-"""
-VIDEO_SERVICE_NAME = "videohub"
-
-
-"""
-" service api version
-"""
-VIDEO_API_VERSION = "1.0.0.1"
-
-
-"""
-" api id
-"""
-VIDEO_API_ID_GETIMAGESAMPLE = 1001

unitree_sdk2py/b2/video/video_client.py

@@ -1,23 +0,0 @@
-import json
-
-from ...rpc.client import Client
-from .video_api import *
-
-
-"""
-" class VideoClient
-"""
-class VideoClient(Client):
-    def __init__(self):
-        super().__init__(VIDEO_SERVICE_NAME, False)
-
-
-    def Init(self):
-        # set api version
-        self._SetApiVerson(VIDEO_API_VERSION)
-        # regist api
-        self._RegistApi(VIDEO_API_ID_GETIMAGESAMPLE, 0)
-
-    # 1001
-    def GetImageSample(self):
-        return self._CallBinary(VIDEO_API_ID_GETIMAGESAMPLE, [])

unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py

@@ -0,0 +1,29 @@
+"""
+" service name
+"""
+MOTION_SWITCHER_SERVICE_NAME = "motion_switcher"
+
+
+"""
+" service api version
+"""
+MOTION_SWITCHER_API_VERSION = "1.0.0.1"
+
+
+"""
+" api id
+"""
+MOTION_SWITCHER_API_ID_CHECK_MODE = 1001
+MOTION_SWITCHER_API_ID_SELECT_MODE = 1002
+MOTION_SWITCHER_API_ID_RELEASE_MODE = 1003
+MOTION_SWITCHER_API_ID_SET_SILENT = 1004
+MOTION_SWITCHER_API_ID_GET_SILENT = 1005
+
+# """
+# " error code
+# """
+# # client side
+# SPORT_ERR_CLIENT_POINT_PATH = 4101
+# # server side
+# SPORT_ERR_SERVER_OVERTIME = 4201
+# SPORT_ERR_SERVER_NOT_INIT = 4202

unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py

@@ -0,0 +1,51 @@
+import json
+
+from ...rpc.client import Client
+from .motion_switcher_api import *
+
+"""
+" class MotionSwitcherClient
+"""
+class MotionSwitcherClient(Client):
+    def __init__(self):
+        super().__init__(MOTION_SWITCHER_SERVICE_NAME, False)
+
+
+    def Init(self):
+        # set api version
+        self._SetApiVerson(MOTION_SWITCHER_API_VERSION)
+        
+        # regist api
+        self._RegistApi(MOTION_SWITCHER_API_ID_CHECK_MODE, 0)
+        self._RegistApi(MOTION_SWITCHER_API_ID_SELECT_MODE, 0)
+        self._RegistApi(MOTION_SWITCHER_API_ID_RELEASE_MODE, 0)
+        self._RegistApi(MOTION_SWITCHER_API_ID_SET_SILENT, 0)
+        self._RegistApi(MOTION_SWITCHER_API_ID_GET_SILENT, 0)
+
+    # 1001
+    def CheckMode(self):
+        p = {}
+        parameter = json.dumps(p)
+        code, data = self._Call(MOTION_SWITCHER_API_ID_CHECK_MODE, parameter)
+        if code == 0:
+            return code, json.loads(data)
+        else:
+            return code, None
+
+    # 1002
+    def SelectMode(self, nameOrAlias):
+        p = {}
+        p["name"] = nameOrAlias
+        parameter = json.dumps(p)
+        code, data = self._Call(MOTION_SWITCHER_API_ID_SELECT_MODE, parameter)
+      
+        return code, None
+
+    # 1003
+    def ReleaseMode(self):
+        p = {}
+        parameter = json.dumps(p)
+        code, data = self._Call(MOTION_SWITCHER_API_ID_RELEASE_MODE, parameter)
+      
+        return code, None
+    

unitree_sdk2py/g1/audio/g1_audio_api.py

@@ -0,0 +1,24 @@
+"""
+" service name
+"""
+AUDIO_SERVICE_NAME = "voice"
+
+"""
+" service api version
+"""
+AUDIO_API_VERSION = "1.0.0.0"
+
+"""
+" api id
+"""
+ROBOT_API_ID_AUDIO_TTS = 1001
+ROBOT_API_ID_AUDIO_ASR = 1002
+ROBOT_API_ID_AUDIO_START_PLAY = 1003
+ROBOT_API_ID_AUDIO_STOP_PLAY = 1004
+ROBOT_API_ID_AUDIO_GET_VOLUME = 1005
+ROBOT_API_ID_AUDIO_SET_VOLUME = 1006 
+ROBOT_API_ID_AUDIO_SET_RGB_LED = 1010
+
+"""
+" error code
+"""

unitree_sdk2py/g1/audio/g1_audio_client.py

@@ -0,0 +1,62 @@
+import json
+
+from ...rpc.client import Client
+from .g1_audio_api import *
+
+"""
+" class SportClient
+"""
+class AudioClient(Client):
+    def __init__(self):
+        super().__init__(AUDIO_SERVICE_NAME, False)
+        self.tts_index = 0
+
+    def Init(self):
+        # set api version
+        self._SetApiVerson(AUDIO_API_VERSION)
+
+        # regist api
+        self._RegistApi(ROBOT_API_ID_AUDIO_TTS, 0)
+        self._RegistApi(ROBOT_API_ID_AUDIO_ASR, 0)
+        self._RegistApi(ROBOT_API_ID_AUDIO_START_PLAY, 0)
+        self._RegistApi(ROBOT_API_ID_AUDIO_STOP_PLAY, 0)
+        self._RegistApi(ROBOT_API_ID_AUDIO_GET_VOLUME, 0)
+        self._RegistApi(ROBOT_API_ID_AUDIO_SET_VOLUME, 0) 
+        self._RegistApi(ROBOT_API_ID_AUDIO_SET_RGB_LED, 0) 
+
+    ## API Call ##
+    def TtsMaker(self, text: str, speaker_id: int):
+        self.tts_index += self.tts_index
+        p = {}
+        p["index"] = self.tts_index
+        p["text"] = text
+        p["speaker_id"] = speaker_id
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_AUDIO_TTS, parameter)
+        return code
+
+    def GetVolume(self):
+        p = {}
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_AUDIO_GET_VOLUME, parameter)
+        if code == 0:
+            return code, json.loads(data)
+        else:
+            return code, None
+
+    def SetVolume(self, volume: int):
+        p = {}
+        p["volume"] = volume
+        # p["name"] = 'volume'
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_AUDIO_SET_VOLUME, parameter)
+        return code
+
+    def LedControl(self, R: int, G: int, B: int):
+        p = {}
+        p["R"] = R
+        p["G"] = G
+        p["B"] = B
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_AUDIO_SET_RGB_LED, parameter)
+        return code

unitree_sdk2py/g1/loco/g1_loco_api.py

@@ -0,0 +1,32 @@
+"""
+" service name
+"""
+LOCO_SERVICE_NAME = "loco"
+
+
+"""
+" service api version
+"""
+LOCO_API_VERSION = "1.0.0.0"
+
+
+"""
+" api id
+"""
+ROBOT_API_ID_LOCO_GET_FSM_ID = 7001
+ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002
+ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 7003
+ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 7004
+ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 7005
+ROBOT_API_ID_LOCO_GET_PHASE = 7006 # deprecated
+
+ROBOT_API_ID_LOCO_SET_FSM_ID = 7101
+ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102
+ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103
+ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104
+ROBOT_API_ID_LOCO_SET_VELOCITY = 7105
+ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106
+
+"""
+" error code
+"""

unitree_sdk2py/g1/loco/g1_loco_client.py

@@ -0,0 +1,124 @@
+import json
+
+from ...rpc.client import Client
+from .g1_loco_api import *
+
+"""
+" class SportClient
+"""
+class LocoClient(Client):
+    def __init__(self):
+        super().__init__(LOCO_SERVICE_NAME, False)
+        self.first_shake_hand_stage_ = -1
+
+    def Init(self):
+        # set api version
+        self._SetApiVerson(LOCO_API_VERSION)
+
+        # regist api
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_ID, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_MODE, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_PHASE, 0) # deprecated
+
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_FSM_ID, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_VELOCITY, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_ARM_TASK, 0)
+
+    # 7101
+    def SetFsmId(self, fsm_id: int):
+        p = {}
+        p["data"] = fsm_id
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter)
+        return code
+
+    # 7102
+    def SetBalanceMode(self, balance_mode: int):
+        p = {}
+        p["data"] = balance_mode
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, parameter)
+        return code
+
+    # 7104
+    def SetStandHeight(self, stand_height: float):
+        p = {}
+        p["data"] = stand_height
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter)
+        return code
+
+    # 7105
+    def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0):
+        p = {}
+        velocity = [vx,vy,omega]
+        p["velocity"] = velocity
+        p["duration"] = duration
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter)
+        return code
+    
+    # 7106
+    def SetTaskId(self, task_id: float):
+        p = {}
+        p["data"] = task_id
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter)
+        return code
+
+    def Damp(self):
+        self.SetFsmId(1)
+    
+    def Start(self):
+        self.SetFsmId(200)
+
+    def Squat(self):
+        self.SetFsmId(2)
+
+    def Sit(self):
+        self.SetFsmId(3)
+
+    def StandUp(self):
+        self.SetFsmId(4)
+
+    def ZeroTorque(self):
+        self.SetFsmId(0)
+
+    def StopMove(self):
+        self.SetVelocity(0., 0., 0.)
+
+    def HighStand(self):
+        UINT32_MAX = (1 << 32) - 1
+        self.SetStandHeight(UINT32_MAX)
+
+    def LowStand(self):
+        UINT32_MIN = 0
+        self.SetStandHeight(UINT32_MIN)
+
+    def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False):
+        duration = 864000.0 if continous_move else 1
+        self.SetVelocity(vx, vy, vyaw, duration)
+
+    def BalanceStand(self, balance_mode: int):
+        self.SetBalanceMode(balance_mode)
+
+    def WaveHand(self, turn_flag: bool = False):
+        self.SetTaskId(1 if turn_flag else 0)
+
+    def ShakeHand(self, stage: int = -1):
+        if stage == 0:
+            self.first_shake_hand_stage_ = False
+            self.SetTaskId(2)
+        elif stage == 1:
+            self.first_shake_hand_stage_ = True
+            self.SetTaskId(3)
+        else:
+            self.first_shake_hand_stage_ = not self.first_shake_hand_stage_
+            return self.SetTaskId(3 if self.first_shake_hand_stage_ else 2)
+    

unitree_sdk2py/go2/sport/sport_api.py

@@ -49,7 +49,20 @@ SPORT_API_ID_WIGGLEHIPS = 1033
 SPORT_API_ID_GETSTATE = 1034
 SPORT_API_ID_ECONOMICGAIT = 1035
 SPORT_API_ID_HEART = 1036
+ROBOT_SPORT_API_ID_DANCE3             = 1037
+ROBOT_SPORT_API_ID_DANCE4             = 1038
+ROBOT_SPORT_API_ID_HOPSPINLEFT        = 1039
+ROBOT_SPORT_API_ID_HOPSPINRIGHT       = 1040
 
+ROBOT_SPORT_API_ID_LEFTFLIP           = 1042
+ROBOT_SPORT_API_ID_BACKFLIP           = 1044
+ROBOT_SPORT_API_ID_FREEWALK           = 1045
+ROBOT_SPORT_API_ID_FREEBOUND          = 1046
+ROBOT_SPORT_API_ID_FREEJUMP           = 1047
+ROBOT_SPORT_API_ID_FREEAVOID          = 1048
+ROBOT_SPORT_API_ID_WALKSTAIR          = 1049
+ROBOT_SPORT_API_ID_WALKUPRIGHT        = 1050
+ROBOT_SPORT_API_ID_CROSSSTEP          = 1051
 
 """
 " error code

unitree_sdk2py/go2/sport/sport_client.py

@@ -73,6 +73,16 @@ class SportClient(Client):
         self._RegistApi(SPORT_API_ID_ECONOMICGAIT, 0)
         self._RegistApi(SPORT_API_ID_HEART, 0)
 
+        self._RegistApi(ROBOT_SPORT_API_ID_LEFTFLIP, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_BACKFLIP, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_FREEWALK, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_FREEBOUND, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_FREEJUMP, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_FREEAVOID, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_WALKSTAIR, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_WALKUPRIGHT, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_CROSSSTEP, 0)
+
     # 1001
     def Damp(self):
         p = {}
@@ -361,4 +371,76 @@ class SportClient(Client):
         p = {}
         parameter = json.dumps(p)
         code, data = self._Call(SPORT_API_ID_HEART, parameter)
+        return code
+    
+    # 1042
+    def LeftFlip(self):
+        p = {}
+        p["data"] = True
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_LEFTFLIP, parameter)
+        return code
+
+    # 1044
+    def BackFlip(self):
+        p = {}
+        p["data"] = True
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_BACKFLIP, parameter)
+        return code
+
+    # 1045
+    def FreeWalk(self, flag: bool):
+        p = {}
+        p["data"] = True
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_FREEWALK, parameter)
+        return code
+
+    # 1046
+    def FreeBound(self, flag: bool):
+        p = {}
+        p["data"] = flag
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_FREEBOUND, parameter)
+        return code
+    
+    # 1047
+    def FreeJump(self, flag: bool):
+        p = {}
+        p["data"] = flag
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_FREEJUMP, parameter)
+        return code
+
+    # 1048
+    def FreeAvoid(self, flag: bool):
+        p = {}
+        p["data"] = flag
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_FREEAVOID, parameter)
+        return code
+
+    # 1049
+    def WalkStair(self, flag: bool):
+        p = {}
+        p["data"] = flag
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_WALKSTAIR, parameter)
+        return code
+    
+    # 1050
+    def WalkUpright(self, flag: bool):
+        p = {}
+        p["data"] = flag
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_WALKUPRIGHT, parameter)
+        return code
+
+    # 1051
+    def CrossStep(self, flag: bool):  
+        p = {}
+        p["data"] = flag
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_SPORT_API_ID_CROSSSTEP, parameter)
         return code

unitree_sdk2py/h1/loco/h1_loco_api.py

@@ -0,0 +1,31 @@
+"""
+" service name
+"""
+LOCO_SERVICE_NAME = "loco"
+
+
+"""
+" service api version
+"""
+LOCO_API_VERSION = "2.0.0.0"
+
+
+"""
+" api id
+"""
+ROBOT_API_ID_LOCO_GET_FSM_ID = 8001
+ROBOT_API_ID_LOCO_GET_FSM_MODE = 8002
+ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 8003
+ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 8004
+ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 8005
+ROBOT_API_ID_LOCO_GET_PHASE = 8006 # deprecated
+
+ROBOT_API_ID_LOCO_SET_FSM_ID = 8101
+ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 8102
+ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 8103
+ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 8104
+ROBOT_API_ID_LOCO_SET_VELOCITY = 8105
+
+"""
+" error code
+"""

unitree_sdk2py/h1/loco/h1_loco_client.py

@@ -0,0 +1,83 @@
+import json
+
+from ...rpc.client import Client
+from .h1_loco_api import *
+
+"""
+" class SportClient
+"""
+class LocoClient(Client):
+    def __init__(self):
+        super().__init__(LOCO_SERVICE_NAME, False)
+
+
+    def Init(self):
+        # set api version
+        self._SetApiVerson(LOCO_API_VERSION)
+
+        # regist api
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_ID, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_MODE, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_GET_PHASE, 0) # deprecated
+
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_FSM_ID, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, 0)
+        self._RegistApi(ROBOT_API_ID_LOCO_SET_VELOCITY, 0)
+
+    # 8101
+    def SetFsmId(self, fsm_id: int):
+        p = {}
+        p["data"] = fsm_id
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter)
+        return code
+
+    # 8104
+    def SetStandHeight(self, stand_height: float):
+        p = {}
+        p["data"] = stand_height
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter)
+        return code
+
+    # 8105
+    def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0):
+        p = {}
+        velocity = [vx,vy,omega]
+        p["velocity"] = velocity
+        p["duration"] = duration
+        parameter = json.dumps(p)
+        code, data = self._Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter)
+        return code
+
+    def Damp(self):
+        self.SetFsmId(1)
+    
+    def Start(self):
+        self.SetFsmId(204)
+
+    def StandUp(self):
+        self.SetFsmId(2)
+
+    def ZeroTorque(self):
+        self.SetFsmId(0)
+
+    def StopMove(self):
+        self.SetVelocity(0., 0., 0.)
+
+    def HighStand(self):
+        UINT32_MAX = (1 << 32) - 1
+        self.SetStandHeight(UINT32_MAX)
+
+    def LowStand(self):
+        UINT32_MIN = 0
+        self.SetStandHeight(UINT32_MIN)
+
+    def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False):
+        duration = 864000.0 if continous_move else 1
+        self.SetVelocity(vx, vy, vyaw, duration)

unitree_sdk2py/idl/default.py

@@ -226,13 +226,16 @@ def unitree_hg_msg_dds__LowState_():
 
 def unitree_hg_msg_dds__PressSensorState_():
     return HGPressSensorState_([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+                               [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 0, 0)
 
 def unitree_hg_msg_dds__HandCmd_():
-    return HGHandCmd_([])
+    return HGHandCmd_([unitree_hg_msg_dds__MotorCmd_() for i in range(7)], [0, 0, 0, 0])
 
 def unitree_hg_msg_dds__HandState_():
-    return HGHandState([], unitree_hg_msg_dds__IMUState_(), [], 0.0, 0.0, [0, 0])
+    return HGHandState_([unitree_hg_msg_dds__MotorState_() for i in range(7)], 
+                        [unitree_hg_msg_dds__PressSensorState_() for i in range(7)],
+                         unitree_hg_msg_dds__IMUState_(), 
+                         0.0, 0.0, 0.0, 0.0, [0, 0], [0, 0])
 
 
 """

unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py

@@ -0,0 +1,23 @@
+"""
+  Generated by Eclipse Cyclone DDS idlc Python Backend
+  Cyclone DDS IDL version: v0.11.0
+  Module: unitree_go.msg.dds_
+  IDL file: MotorCmds_.idl
+
+"""
+
+from enum import auto
+from typing import TYPE_CHECKING, Optional
+from dataclasses import dataclass, field
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class MotorCmds_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorCmds_"):
+    cmds: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorCmd_'] = field(default_factory=lambda: [])
+
+

unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py

@@ -0,0 +1,23 @@
+"""
+  Generated by Eclipse Cyclone DDS idlc Python Backend
+  Cyclone DDS IDL version: v0.11.0
+  Module: unitree_go.msg.dds_
+  IDL file: MotorStates_.idl
+
+"""
+
+from enum import auto
+from typing import TYPE_CHECKING, Optional
+from dataclasses import dataclass, field
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class MotorStates_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorStates_"):
+    states: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorState_'] = field(default_factory=lambda: [])
+
+

unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py

@@ -17,7 +17,9 @@ from ._LidarState_ import LidarState_
 from ._LowCmd_ import LowCmd_
 from ._LowState_ import LowState_
 from ._MotorCmd_ import MotorCmd_
+from ._MotorCmds_ import MotorCmds_
 from ._MotorState_ import MotorState_
+from ._MotorStates_ import MotorStates_
 from ._Req_ import Req_
 from ._Res_ import Res_
 from ._SportModeState_ import SportModeState_
@@ -26,4 +28,4 @@ from ._PathPoint_ import PathPoint_
 from ._UwbState_ import UwbState_
 from ._UwbSwitch_ import UwbSwitch_
 from ._WirelessController_ import WirelessController_
-__all__ = ["AudioData_", "BmsCmd_", "BmsState_", "Error_", "Go2FrontVideoData_", "HeightMap_", "IMUState_", "InterfaceConfig_", "LidarState_", "LowCmd_", "LowState_", "MotorCmd_", "MotorState_", "Req_", "Res_", "SportModeState_", "TimeSpec_", "PathPoint_",  "UwbState_", "UwbSwitch_", "WirelessController_", ]
+__all__ = ["AudioData_", "BmsCmd_", "BmsState_", "Error_", "Go2FrontVideoData_", "HeightMap_", "IMUState_", "InterfaceConfig_", "LidarState_", "LowCmd_", "LowState_", "MotorCmd_", "MotorCmds_", "MotorState_", "MotorStates_", "Req_", "Res_", "SportModeState_", "TimeSpec_", "PathPoint_",  "UwbState_", "UwbSwitch_", "WirelessController_", ]

unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py

@@ -23,5 +23,6 @@ import cyclonedds.idl.types as types
 @annotate.autoid("sequential")
 class HandCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.HandCmd_"):
     motor_cmd: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorCmd_']
+    reserve: types.array[types.uint32, 4]
 
 

unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py

@@ -23,10 +23,13 @@ import cyclonedds.idl.types as types
 @annotate.autoid("sequential")
 class HandState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.HandState_"):
     motor_state: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorState_']
-    imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_'
     press_sensor_state: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.PressSensorState_']
+    imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_'
     power_v: types.float32
     power_a: types.float32
+    system_v: types.float32
+    device_v: types.float32
+    error: types.array[types.uint32, 2]
     reserve: types.array[types.uint32, 2]
 
 

unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py

@@ -24,5 +24,7 @@ import cyclonedds.idl.types as types
 class PressSensorState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.PressSensorState_"):
     pressure: types.array[types.float32, 12]
     temperature: types.array[types.float32, 12]
+    lost: types.uint32
+    reserve: types.uint32
 
 

unitree_sdk2py/utils/joystick.py

@@ -0,0 +1,251 @@
+import math
+import struct
+import os
+os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = "hide" # Disable pygame welcome message
+import pygame
+import time
+
+class Button:
+  def __init__(self) -> None:
+    self.pressed = False
+    self.on_pressed = False
+    self.on_released = False
+    self.data = 0
+    self.click_count = 0  # 记录连续点击次数
+    self.last_pressed_time = 0  # 上次按下时间
+
+  def __call__(self, data) -> None:
+    current_time = time.perf_counter()
+    # print('before',self.data)
+
+    self.pressed = (data != 0)
+    self.on_pressed = self.pressed and self.data == 0
+    self.on_released = not self.pressed and self.data != 0
+
+    # print('after',self.data)
+            # 处理连续点击
+    if self.on_pressed:
+        # print('on_pressed')
+        # print('on_pressed current_time',current_time)
+        # print('on_pressed last_pressed_time',self.last_pressed_time)
+        # print('on_pressed diff',current_time-self.last_pressed_time)
+
+        if current_time - self.last_pressed_time <= 0.3:  # 0.1 秒以内的连续点击
+            self.click_count += 1
+            # print(self.click_count)
+        else:
+            self.click_count = 0  # 超过时间间隔，重置计数器
+        self.last_pressed_time = current_time
+    self.data = data
+    
+  def reset_click_count(self):
+        """手动重置连续点击计数器"""
+        self.click_count = 0
+
+class Axis:
+  def __init__(self) -> None:
+    self.data = 0.0
+    self.pressed = False
+    self.on_pressed = False
+    self.on_released = False
+  
+    self.smooth = 0.03
+    self.deadzone = 0.01
+    self.threshold = 0.5
+
+  def __call__(self, data) -> None:
+    data_deadzone = 0.0 if math.fabs(data) < self.deadzone else data
+    new_data = self.data * (1 - self.smooth) + data_deadzone * self.smooth
+    self.pressed = math.fabs(new_data) > self.threshold
+    self.on_pressed = self.pressed and math.fabs(self.data) < self.threshold
+    self.on_released = not self.pressed and math.fabs(self.data) > self.threshold
+    self.data = new_data
+
+
+class Joystick:
+  def __init__(self) -> None:
+    # Buttons
+    self.back = Button()
+    self.start = Button()
+    # self.LS = Button()
+    # self.RS = Button()
+    self.LB = Button()
+    self.RB = Button()
+    self.LT = Button()
+    self.RT = Button()
+    self.A = Button()
+    self.B = Button()
+    self.X = Button()
+    self.Y = Button()
+    self.up = Button()
+    self.down = Button()
+    self.left = Button()
+    self.right = Button()
+    self.F1 = Button()
+    self.F2 = Button()
+
+    # Axes
+    # self.LT = Axis()
+    # self.RT = Axis()
+    self.lx = Axis()
+    self.ly = Axis()
+    self.rx = Axis()
+    self.ry = Axis()
+    
+    self.last_active_time = time.perf_counter()  # 最后一次活动时间
+    self.inactive_timeout = 0.5  # 超时时间（单位：秒）
+  def update(self):
+    """
+    Update the current handle key based on the original data
+    Used to update flag bits such as on_pressed
+
+    Examples:
+    >>> new_A_data = 1
+    >>> self.A( new_A_data )
+    """
+    pass
+
+  def extract(self, wireless_remote):
+    """
+    Extract data from unitree_joystick
+    wireless_remote: uint8_t[40]
+    """
+    # Buttons
+    button1 = [int(data) for data in f'{wireless_remote[2]:08b}']
+    button2 = [int(data) for data in f'{wireless_remote[3]:08b}']
+    self.LT(button1[2])
+    self.RT(button1[3])
+    self.back(button1[4])
+    self.start(button1[5])
+    self.LB(button1[6])
+    self.RB(button1[7])
+    self.left(button2[0])    
+    self.down(button2[1])
+    self.right(button2[2])
+    self.up(button2[3])
+    self.Y(button2[4])
+    self.X(button2[5])
+    self.B(button2[6])
+    self.A(button2[7])
+    # Axes
+    self.lx( struct.unpack('f', bytes(wireless_remote[4:8]))[0] )
+    self.rx( struct.unpack('f', bytes(wireless_remote[8:12]))[0] )
+    self.ry( struct.unpack('f', bytes(wireless_remote[12:16]))[0] )
+    self.ly( struct.unpack('f', bytes(wireless_remote[20:24]))[0] )
+    
+    
+    # 检查是否有按键按下
+    if any([
+        self.LT.pressed, self.RT.pressed, self.back.pressed, self.start.pressed,
+        self.LB.pressed, self.RB.pressed, self.left.pressed, self.down.pressed,
+        self.right.pressed, self.up.pressed, self.Y.pressed, self.X.pressed,
+        self.B.pressed, self.A.pressed
+    ]):
+        self.last_active_time = time.perf_counter()  # 更新最后一次活动时间
+    elif time.perf_counter() - self.last_active_time > self.inactive_timeout:
+        # 超过设定的超时时间未按下任何键，重置所有按键的点击计数
+        self.reset_all_click_counts()
+        self.last_active_time = time.perf_counter()  # 重置最后活动时间
+
+  def reset_all_click_counts(self):
+        """重置所有按键的连续点击计数器"""
+        for button in [
+            self.LT, self.RT, self.back, self.start, self.LB, self.RB,
+            self.left, self.down, self.right, self.up, self.Y, self.X, self.B, self.A
+        ]:
+            button.reset_click_count()
+        
+  def combine(self):
+    """
+    Merge data from Joystick to wireless_remote    
+    """
+    # prepare an empty list
+    wireless_remote = [0 for _ in range(40)]
+
+    # Buttons
+    wireless_remote[2] = int(''.join([f'{key}' for key in [
+      0, 0, round(self.LT.data), round(self.RT.data), 
+      self.back.data, self.start.data, self.LB.data, self.RB.data,
+    ]]), 2)
+    wireless_remote[3] = int(''.join([f'{key}' for key in [
+      self.left.data, self.down.data, self.right.data, 
+      self.up.data, self.Y.data, self.X.data, self.B.data, self.A.data,
+    ]]), 2)
+
+    # Axes
+    sticks = [self.lx.data, self.rx.data, self.ry.data, self.ly.data]
+    packs = list(map(lambda x: struct.pack('f', x), sticks))
+    wireless_remote[4:8] = packs[0]
+    wireless_remote[8:12] = packs[1]
+    wireless_remote[12:16] = packs[2]
+    wireless_remote[20:24] = packs[3]
+    return wireless_remote
+
+class PyGameJoystick(Joystick):
+  def __init__(self) -> None:
+    super().__init__()
+
+    pygame.init()
+    pygame.joystick.init()
+    if pygame.joystick.get_count() <= 0:
+      raise Exception("No joystick found!")
+    
+    self._joystick = pygame.joystick.Joystick(0)
+    self._joystick.init()
+
+  def print(self):
+    print("\naxes: ")
+    for i in range(self._joystick.get_numaxes()):
+      print(self._joystick.get_axis(i), end=" ")
+    print("\nbuttons: ")
+    for i in range(self._joystick.get_numbuttons()):
+      print(self._joystick.get_button(i), end=" ")
+    print("\nhats: ")
+    for i in range(self._joystick.get_numhats()):
+      print(self._joystick.get_hat(i), end=" ")
+    print("\nballs: ")
+    for i in range(self._joystick.get_numballs()):
+      print(self._joystick.get_ball(i), end=" ")
+    print("\n")
+
+class LogicJoystick(PyGameJoystick):
+  """ Logic F710 """
+  def __init__(self) -> None:
+    super().__init__()
+
+  def update(self):
+    pygame.event.pump()
+
+    self.back(self._joystick.get_button(6))
+    self.start(self._joystick.get_button(7))
+    self.LS(self._joystick.get_button(9))
+    self.RS(self._joystick.get_button(10))
+    self.LB(self._joystick.get_button(4))
+    self.RB(self._joystick.get_button(5))
+    self.A(self._joystick.get_button(0))
+    self.B(self._joystick.get_button(1))
+    self.X(self._joystick.get_button(2))
+    self.Y(self._joystick.get_button(3))
+
+    self.LT((self._joystick.get_axis(2) + 1)/2)
+    self.RT((self._joystick.get_axis(5) + 1)/2)
+    self.rx(self._joystick.get_axis(3))
+    self.ry(-self._joystick.get_axis(4))
+
+
+    # Logitech controller has 2 modes
+    # mode 1: light down
+    self.up(1 if self._joystick.get_hat(0)[1] > 0.5 else 0)
+    self.down(1 if self._joystick.get_hat(0)[1] < -0.5 else 0)
+    self.left(1 if self._joystick.get_hat(0)[0] < -0.5 else 0)
+    self.right(1 if self._joystick.get_hat(0)[0] > 0.5 else 0)
+    self.lx(self._joystick.get_axis(0))
+    self.ly(-self._joystick.get_axis(1))
+    # mode 2: light up
+    # self.up(1 if self._joystick.get_axis(1) < -0.5 else 0)
+    # self.down(1 if self._joystick.get_axis(0) > 0.5 else 0)
+    # self.left(1 if self._joystick.get_axis(0) < -0.5 else 0)
+    # self.right(1 if self._joystick.get_axis(0) > 0.5 else 0)
+    # self.lx(self._joystick.get_hat(0)[1])
+    # self.ly(self._joystick.get_hat(0)[1])
+