add unitree_sdk2py

Author: yangning wu

.gitignore

@@ -0,0 +1,36 @@
+# Generated by MacOS
+.DS_Store
+
+# Generated by Windows
+Thumbs.db
+
+# Applications
+*.app
+*.exe
+*.war
+
+# Large media files
+*.mp4
+*.tiff
+*.avi
+*.flv
+*.mov
+*.wmv
+*.jpg
+*.png
+
+# VS Code
+.vscode
+
+# other
+*.egg-info
+__pycache__
+
+# IDEs
+.idea
+
+# cache
+.pytest_cache
+
+# JetBrains IDE
+.idea/

README zh.md

@@ -0,0 +1,121 @@
+# unitree_sdk2py
+unitree_sdk2 python 接口
+
+# 安装
+## 依赖
+- python>=3.8
+- cyclonedds==0.10.2
+- numpy
+- opencv-python
+
+## 安装 unitree_sdk2py
+在终端中执行：
+```bash
+cd ~
+sudo apt install python3-pip
+git clone https://github.com/unitreerobotics/unitree_sdk2py.git
+cd unitree_sdk2py
+pip3 install -e .
+```
+## FAQ
+##### 1. `pip3 install -e .` 遇到报错
+```bash
+Could not locate cyclonedds. Try to set CYCLONEDDS_HOME or CMAKE_PREFIX_PATH
+```
+该错误提示找不到 cyclonedds 路径。首先编译安装cyclonedds：
+```bash
+cd ~
+git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x 
+cd cyclonedds && mkdir build install && cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=../install
+cmake --build . --target install
+```
+进入 unitree_sdk2py 目录，设置 `CYCLONEDDS_HOME` 为刚刚编译好的 cyclonedds 所在路径，再安装unitree_sdk2py
+```bash
+cd ~/unitree_sdk2py
+export CYCLONEDDS_HOME="~/cyclonedds/install"
+pip3 install -e .
+```
+
+详细见：
+https://pypi.org/project/cyclonedds/#installing-with-pre-built-binaries
+
+# 使用
+python sdk2 接口与 unitree_skd2的接口保持一致，通过请求响应或订阅发布topic实现机器人的状态获取和控制。相应的例程位于`/example`目录下。在运行例程前，需要根据文档 https://support.unitree.com/home/zh/developer/Quick_start 配置好机器人的网络连接。
+## DDS通讯
+在终端中执行：
+```bash
+python3 ./example/helloworld/publisher.py
+```
+打开新的终端，执行：
+```bash
+python3 ./example/helloworld/subscriber.py
+```
+可以看到终端输出的数据信息。`publisher.py` 和 `subscriber.py` 传输的数据定义在 `user_data.py` 中，用户可以根据需要自行定义需要传输的数据结构。
+
+## 高层状态和控制
+高层接口的数据结构和控制方式与unitree_sdk2一致。具体可见：https://support.unitree.com/home/zh/developer/sports_services
+### 高层状态
+终端中执行：
+```bash
+python3 ./example/high_level/read_highstate.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。
+### 高层控制
+终端中执行：
+```bash
+python3 ./example/high_level/sportmode_test.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。
+该例程提供了几种测试方法，可根据测试需要选择:
+```python
+test.StandUpDown() # 站立趴下
+# test.VelocityMove() # 速度控制
+# test.BalanceAttitude() # 姿态控制
+# test.TrajectoryFollow() # 轨迹跟踪
+# test.SpecialMotions() # 特殊动作
+
+```
+## 底层状态和控制
+底层接口的数据结构和控制方式与unitree_sdk2一致。具体可见：https://support.unitree.com/home/zh/developer/Basic_services
+### 底层状态
+终端中执行：
+```bash
+python3 ./example/low_level/lowlevel_control.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。程序会输出右前腿hip关节的状态、IMU和电池电压信息。
+
+### 底层电机控制
+首先使用 app 关闭高层运动服务(sport_mode)，否则会导致指令冲突。
+终端中执行：
+```bash
+python3 ./example/low_level/lowlevel_control.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。左后腿 hip 关节会保持在0角度 (安全起见，这里设置 kp=10, kd=1)，左后腿 calf 关节将持续输出 1Nm 的转矩。
+
+## 遥控器状态获取
+终端中执行：
+```bash
+python3 ./example/wireless_controller/wireless_controller.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。
+终端将输出每一个按键的状态。对于遥控器按键的定义和数据结构可见： https://support.unitree.com/home/zh/developer/Get_remote_control_status
+
+## 前置摄像头
+使用opencv获取前置摄像头(确保在有图形界面的系统下运行, 按 ESC 退出程序): 
+```bash
+python3 ./example/front_camera/camera_opencv.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。
+
+## 避障开关
+```bash
+python3 ./example/obstacles_avoid_switch/obstacles_avoid_switch.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。机器人将循环开启和关闭避障功能。关于避障服务，详细见 https://support.unitree.com/home/zh/developer/ObstaclesAvoidClient
+
+## 灯光音量控制
+```bash
+python3 ./example/vui_client/vui_client_example.py enp2s0
+```
+其中 `enp2s0` 为机器人所连接的网卡名称，请根据实际情况修改。机器人将循环调节音量和灯光亮度。该接口详细见 https://support.unitree.com/home/zh/developer/VuiClient

README.md

@@ -0,0 +1,112 @@
+# unitree_sdk2py
+Python interface for unitree sdk2
+
+# Installation
+## Dependencies
+- Python >= 3.8
+- cyclonedds == 0.10.2
+- numpy
+- opencv-python
+## Install unitree_sdk2py
+Execute the following commands in the terminal:
+```bash
+cd ~
+sudo apt install python3-pip
+git clone https://github.com/unitreerobotics/unitree_sdk2py.git
+cd unitree_sdk2py
+pip3 install -e .
+```
+## FAQ
+##### 1. Error when `pip3 install -e .`:
+```bash
+Could not locate cyclonedds. Try to set CYCLONEDDS_HOME or CMAKE_PREFIX_PATH
+```
+This error mentions that the cyclonedds path could not be found. First compile and install cyclonedds:
+
+```bash
+cd ~
+git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x 
+cd cyclonedds && mkdir build install && cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=../install
+cmake --build . --target install
+```
+Enter the unitree_sdk2py directory, set `CYCLONEDDS_HOME` to the path of the cyclonedds you just compiled, and then install unitree_sdk2py.
+```bash
+cd ~/unitree_sdk2py
+export CYCLONEDDS_HOME="~/cyclonedds/install"
+pip3 install -e .
+```
+For details, see: https://pypi.org/project/cyclonedds/#installing-with-pre-built-binaries
+
+# Usage
+The Python sdk2 interface maintains consistency with the unitree_sdk2 interface, achieving robot status acquisition and control through request-response or topic subscription/publishing. Example programs are located in the `/example` directory. Before running the examples, configure the robot's network connection as per the instructions in the document at https://support.unitree.com/home/en/developer/Quick_start.
+## DDS Communication
+In the terminal, execute:
+```bash
+python3 ./example/helloworld/publisher.py
+```
+Open a new terminal and execute:
+```bash
+python3 ./example/helloworld/subscriber.py
+```
+You will see the data output in the terminal. The data structure transmitted between `publisher.py` and `subscriber.py` is defined in `user_data.py`, and users can define the required data structure as needed.
+## High-Level Status and Control
+The high-level interface maintains consistency with unitree_sdk2 in terms of data structure and control methods. For detailed information, refer to https://support.unitree.com/home/en/developer/sports_services.
+### High-Level Status
+Execute the following command in the terminal:
+```bash
+python3 ./example/high_level/read_highstate.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected,.
+### High-Level Control
+Execute the following command in the terminal:
+```bash
+python3 ./example/high_level/sportmode_test.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected. This example program provides several test methods, and you can choose the required tests as follows:
+```python
+test.StandUpDown() # Stand up and lie down
+# test.VelocityMove() # Velocity control
+# test.BalanceAttitude() # Attitude control
+# test.TrajectoryFollow() # Trajectory tracking
+# test.SpecialMotions() # Special motions
+```
+## Low-Level Status and Control
+The low-level interface maintains consistency with unitree_sdk2 in terms of data structure and control methods. For detailed information, refer to https://support.unitree.com/home/en/developer/Basic_services.
+### Low-Level Status
+Execute the following command in the terminal:
+```bash
+python3 ./example/low_level/lowlevel_control.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected. The program will output the state of the right front leg hip joint, IMU, and battery voltage.
+### Low-Level Motor Control
+First, use the app to turn off the high-level motion service (sport_mode) to prevent conflicting instructions.
+Execute the following command in the terminal:
+```bash
+python3 ./example/low_level/lowlevel_control.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected. The left hind leg hip joint will maintain a 0-degree position (for safety, set kp=10, kd=1), and the left hind leg calf joint will continuously output 1Nm of torque.
+## Wireless Controller Status
+Execute the following command in the terminal:
+```bash
+python3 ./example/wireless_controller/wireless_controller.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected. The terminal will output the status of each key. For the definition and data structure of the remote control keys, refer to https://support.unitree.com/home/en/developer/Get_remote_control_status.
+## Front Camera
+Use OpenCV to obtain the front camera (ensure to run on a system with a graphical interface, and press ESC to exit the program):
+```bash
+python3 ./example/front_camera/camera_opencv.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected.
+
+## Obstacle Avoidance Switch
+```bash
+python3 ./example/obstacles_avoid_switch/obstacles_avoid_switch.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected. The robot will cycle obstacle avoidance on and off. For details on the obstacle avoidance service, see https://support.unitree.com/home/en/developer/ObstaclesAvoidClient
+
+## Light and volume control
+```bash
+python3 ./example/vui_client/vui_client_example.py enp2s0
+```
+Replace `enp2s0` with the name of the network interface to which the robot is connected.T he robot will cycle the volume and light brightness. The interface is detailed at https://support.unitree.com/home/en/developer/VuiClient

example/front_camera/camera_opencv.py

@@ -0,0 +1,41 @@
+from unitree_sdk2py.core.channel import ChannelFactortyInitialize
+from unitree_sdk2py.go2.video.video_client import VideoClient
+import cv2
+import numpy as np
+import sys
+
+
+if __name__ == "__main__":
+    if len(sys.argv)>1:
+        ChannelFactortyInitialize(0, sys.argv[1])
+    else:
+        ChannelFactortyInitialize(0)
+
+    client = VideoClient()  # Create a video client
+    client.SetTimeout(3.0)
+    client.Init()
+
+    code, data = client.GetImageSample()
+
+    # Request normal when code==0
+    while code == 0:
+        # Get Image data from Go2 robot
+        code, data = client.GetImageSample()
+
+        # Convert to numpy image
+        image_data = np.frombuffer(bytes(data), dtype=np.uint8)
+        image = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
+
+        # Display image
+        cv2.imshow("front_camera", image)
+        # Press ESC to stop
+        if cv2.waitKey(20) == 27:
+            break
+
+    if code != 0:
+        print("Get image sample error. code:", code)
+    else:
+        # Capture an image
+        cv2.imwrite("front_image.jpg", image)
+
+    cv2.destroyWindow("front_camera")

example/front_camera/capture_image.py

@@ -0,0 +1,30 @@
+import time
+import os
+import sys
+
+from unitree_sdk2py.core.channel import ChannelFactortyInitialize
+from unitree_sdk2py.go2.video.video_client import VideoClient
+
+if __name__ == "__main__":
+    if len(sys.argv)>1:
+        ChannelFactortyInitialize(0, sys.argv[1])
+    else:
+        ChannelFactortyInitialize(0)
+
+    client = VideoClient()
+    client.SetTimeout(3.0)
+    client.Init()
+
+    print("##################GetImageSample###################")
+    code, data = client.GetImageSample()
+
+    if code != 0:
+        print("get image sample error. code:", code)
+    else:
+        imageName = "./img.jpg"
+        print("ImageName:", imageName)
+
+        with open(imageName, "+wb") as f:
+            f.write(bytes(data))
+
+    time.sleep(1)

example/helloworld/publisher.py

@@ -0,0 +1,28 @@
+import time
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactortyInitialize
+from user_data import *
+
+
+if __name__ == "__main__":
+    ChannelFactortyInitialize()
+
+    # Create a publisher to publish the data defined in UserData class
+    pub = ChannelPublisher("topic", UserData)
+    pub.Init()
+
+    for i in range(30):
+        # Create a Userdata message
+        msg = UserData(" ", 0)
+        msg.string_data = "Hello world"
+        msg.float_data = time.time()
+
+        # Publish message
+        if pub.Write(msg, 0.5):
+            print("Publish success. msg:", msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    pub.Close()

example/helloworld/subscriber.py

@@ -0,0 +1,20 @@
+import time
+
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactortyInitialize
+from user_data import *
+
+
+if __name__ == "__main__":
+    ChannelFactortyInitialize()
+    # Create a subscriber to subscribe the data defined in UserData class
+    sub = ChannelSubscriber("topic", UserData)
+    sub.Init()
+
+    while True:
+        msg = sub.Read()
+        if msg is not None:
+            print("Subscribe success. msg:", msg)
+        else:
+            print("No data subscribed.")
+            break
+    sub.Close()