first commit
This commit is contained in:
commit
1197f66f90
29
LICENSE
Normal file
29
LICENSE
Normal file
@ -0,0 +1,29 @@
|
||||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2016-2024 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
121
README zh.md
Normal file
121
README zh.md
Normal file
@ -0,0 +1,121 @@
|
||||
# unitree_sdk2_python
|
||||
unitree_sdk2 python 接口
|
||||
|
||||
# 安装
|
||||
## 依赖
|
||||
- python>=3.8
|
||||
- cyclonedds==0.10.2
|
||||
- numpy
|
||||
- opencv-python
|
||||
|
||||
## 安装 unitree_sdk2_python
|
||||
在终端中执行：
|
||||
```bash
|
||||
cd ~
|
||||
sudo apt install python3-pip
|
||||
git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
|
||||
cd unitree_sdk2_python
|
||||
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_sdk2_python 目录，设置 `CYCLONEDDS_HOME` 为刚刚编译好的 cyclonedds 所在路径，再安装 unitree_sdk2_python
|
||||
```bash
|
||||
cd ~/unitree_sdk2_python
|
||||
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
|
||||
118
README.md
Normal file
118
README.md
Normal file
@ -0,0 +1,118 @@
|
||||
# unitree_sdk2_python
|
||||
Python interface for unitree sdk2
|
||||
|
||||
# Installation
|
||||
## Dependencies
|
||||
- Python >= 3.8
|
||||
- cyclonedds == 0.10.2
|
||||
- numpy
|
||||
- opencv-python
|
||||
## Install unitree_sdk2_python
|
||||
|
||||
```bash
|
||||
pip install unitree_sdk2py
|
||||
```
|
||||
|
||||
### Installing from source
|
||||
Execute the following commands in the terminal:
|
||||
```bash
|
||||
cd ~
|
||||
sudo apt install python3-pip
|
||||
git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
|
||||
cd unitree_sdk2_python
|
||||
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_sdk2_python directory, set `CYCLONEDDS_HOME` to the path of the cyclonedds you just compiled, and then install unitree_sdk2_python.
|
||||
```bash
|
||||
cd ~/unitree_sdk2_python
|
||||
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
|
||||
21
doc/运行教程.md
Normal file
21
doc/运行教程.md
Normal file
@ -0,0 +1,21 @@
|
||||
# 1. 机器人进入调试模式
|
||||
- G1 处于悬挂状态 和 阻尼状态（L2+B）
|
||||
- 同时按下遥控器的L2+R2组合键，G1进入调试模式。
|
||||
- 此时按下L2+A， G1的双臂会摆出特定姿势。
|
||||
- 再按L2+B，手臂会落回去。以此来判断是否进入调试模式
|
||||
|
||||
# 2.配置网络
|
||||
- 机器人默认ip 192.168.123.161
|
||||
- 服务系统需要和机器人在同一网段下--推荐ip192.168.123.99
|
||||
- 验证网络是否联通--ping 192.168.123.161
|
||||
|
||||
# 3.安装unitree_sdk2_python
|
||||
- 下载zip包--https://github.com/unitreerobotics/unitree_sdk2_python# 并解压缩
|
||||
- 进入解压缩后的文件中， pip3 install -e .
|
||||
|
||||
# 4.python环境
|
||||
- conda activate unitree_sdk2_python
|
||||
# 4.测试机器人程序
|
||||
- python example/g1/audio/g1_audio_client_example.py enp46s0， enp46s0为和机器人再同一网段的网卡名。
|
||||
|
||||
|
||||
51
example/b2/camera/camera_opencv.py
Normal file
51
example/b2/camera/camera_opencv.py
Normal file
@ -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()
|
||||
|
||||
51
example/b2/camera/capture_image.py
Normal file
51
example/b2/camera/capture_image.py
Normal file
@ -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)
|
||||
105
example/b2/high_level/b2_sport_client.py
Normal file
105
example/b2/high_level/b2_sport_client.py
Normal file
@ -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)
|
||||
175
example/b2/low_level/b2_stand_example.py
Normal file
175
example/b2/low_level/b2_stand_example.py
Normal file
@ -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)
|
||||
20
example/b2/low_level/unitree_legged_const.py
Normal file
20
example/b2/low_level/unitree_legged_const.py
Normal file
@ -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
|
||||
51
example/b2w/camera/camera_opencv.py
Normal file
51
example/b2w/camera/camera_opencv.py
Normal file
@ -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()
|
||||
|
||||
51
example/b2w/camera/capture_image.py
Normal file
51
example/b2w/camera/capture_image.py
Normal file
@ -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)
|
||||
101
example/b2w/high_level/b2w_sport_client.py
Normal file
101
example/b2w/high_level/b2w_sport_client.py
Normal file
@ -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)
|
||||
196
example/b2w/low_level/b2w_stand_example.py
Normal file
196
example/b2w/low_level/b2w_stand_example.py
Normal file
@ -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)
|
||||
24
example/b2w/low_level/unitree_legged_const.py
Normal file
24
example/b2w/low_level/unitree_legged_const.py
Normal file
@ -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
|
||||
44
example/g1/audio/g1_audio_client_example.py
Normal file
44
example/g1/audio/g1_audio_client_example.py
Normal file
@ -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)
|
||||
|
||||
192
example/g1/high_level/g1_arm5_sdk_dds_example.py
Normal file
192
example/g1/high_level/g1_arm5_sdk_dds_example.py
Normal file
@ -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)
|
||||
194
example/g1/high_level/g1_arm7_sdk_dds_example.py
Normal file
194
example/g1/high_level/g1_arm7_sdk_dds_example.py
Normal file
@ -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)
|
||||
141
example/g1/high_level/g1_arm_action_example.py
Normal file
141
example/g1/high_level/g1_arm_action_example.py
Normal file
@ -0,0 +1,141 @@
|
||||
import time
|
||||
import sys
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.g1.arm.g1_arm_action_client import G1ArmActionClient
|
||||
from unitree_sdk2py.g1.arm.g1_arm_action_client import action_map
|
||||
from dataclasses import dataclass
|
||||
|
||||
@dataclass
|
||||
class TestOption:
|
||||
name: str
|
||||
id: int
|
||||
|
||||
option_list = [
|
||||
TestOption(name="release arm", id=0),
|
||||
TestOption(name="shake hand", id=1),
|
||||
TestOption(name="high five", id=2),
|
||||
TestOption(name="hug", id=3),
|
||||
TestOption(name="high wave", id=4),
|
||||
TestOption(name="clap", id=5),
|
||||
TestOption(name="face wave", id=6),
|
||||
TestOption(name="left kiss", id=7),
|
||||
TestOption(name="heart", id=8),
|
||||
TestOption(name="right heart", id=9),
|
||||
TestOption(name="hands up", id=10),
|
||||
TestOption(name="x-ray", id=11),
|
||||
TestOption(name="right hand up", id=12),
|
||||
TestOption(name="reject", id=13),
|
||||
TestOption(name="right kiss", id=14),
|
||||
TestOption(name="two-hand kiss", id=15),
|
||||
|
||||
]
|
||||
|
||||
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
|
||||
|
||||
armAction_client = G1ArmActionClient()
|
||||
armAction_client.SetTimeout(10.0)
|
||||
armAction_client.Init()
|
||||
|
||||
# actionList = armAction_client.GetActionList()
|
||||
# print("actionList\n",actionList)
|
||||
|
||||
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:
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 1:
|
||||
armAction_client.ExecuteAction(action_map.get("shake hand"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 2:
|
||||
armAction_client.ExecuteAction(action_map.get("high five"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 3:
|
||||
armAction_client.ExecuteAction(action_map.get("hug"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 4:
|
||||
armAction_client.ExecuteAction(action_map.get("high wave"))
|
||||
elif test_option.id == 5:
|
||||
armAction_client.ExecuteAction(action_map.get("clap"))
|
||||
elif test_option.id == 6:
|
||||
armAction_client.ExecuteAction(action_map.get("face wave"))
|
||||
elif test_option.id == 7:
|
||||
armAction_client.ExecuteAction(action_map.get("left kiss"))
|
||||
elif test_option.id == 8:
|
||||
armAction_client.ExecuteAction(action_map.get("heart"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 9:
|
||||
armAction_client.ExecuteAction(action_map.get("right heart"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 10:
|
||||
armAction_client.ExecuteAction(action_map.get("hands up"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 11:
|
||||
armAction_client.ExecuteAction(action_map.get("x-ray"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 12:
|
||||
armAction_client.ExecuteAction(action_map.get("right hand up"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 13:
|
||||
armAction_client.ExecuteAction(action_map.get("reject"))
|
||||
time.sleep(2)
|
||||
armAction_client.ExecuteAction(action_map.get("release arm"))
|
||||
elif test_option.id == 14:
|
||||
armAction_client.ExecuteAction(action_map.get("right kiss"))
|
||||
elif test_option.id == 15:
|
||||
armAction_client.ExecuteAction(action_map.get("two-hand kiss"))
|
||||
|
||||
time.sleep(1)
|
||||
117
example/g1/high_level/g1_loco_client_example.py
Normal file
117
example/g1/high_level/g1_loco_client_example.py
Normal file
@ -0,0 +1,117 @@
|
||||
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="Squat2StandUp", id=1),
|
||||
TestOption(name="StandUp2Squat", 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),
|
||||
TestOption(name="Lie2StandUp", id=12),
|
||||
]
|
||||
|
||||
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.Damp()
|
||||
time.sleep(0.5)
|
||||
sport_client.Squat2StandUp()
|
||||
elif test_option.id == 2:
|
||||
sport_client.StandUp2Squat()
|
||||
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()
|
||||
elif test_option.id == 12:
|
||||
sport_client.Damp()
|
||||
time.sleep(0.5)
|
||||
sport_client.Lie2StandUp() # When using the Lie2StandUp function, ensure that the robot faces up and the ground is hard, flat and rough.
|
||||
|
||||
time.sleep(1)
|
||||
205
example/g1/low_level/g1_low_level_example.py
Normal file
205
example/g1/low_level/g1_low_level_example.py
Normal file
@ -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)
|
||||
5
example/g1/readme.md
Normal file
5
example/g1/readme.md
Normal file
@ -0,0 +1,5 @@
|
||||
This example is a test of Unitree G1/H1-2 robot.
|
||||
|
||||
**Note:**
|
||||
idl/unitree_go is used for Unitree Go2/B2/H1/B2w/Go2w robots
|
||||
idl/unitree_hg is used for Unitree G1/H1-2 robots
|
||||
41
example/go2/front_camera/camera_opencv.py
Normal file
41
example/go2/front_camera/camera_opencv.py
Normal file
@ -0,0 +1,41 @@
|
||||
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
|
||||
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:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(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")
|
||||
30
example/go2/front_camera/capture_image.py
Normal file
30
example/go2/front_camera/capture_image.py
Normal file
@ -0,0 +1,30 @@
|
||||
import time
|
||||
import os
|
||||
import sys
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
|
||||
from unitree_sdk2py.go2.video.video_client import VideoClient
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(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)
|
||||
170
example/go2/high_level/go2_sport_client.py
Normal file
170
example/go2/high_level/go2_sport_client.py
Normal file
@ -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)
|
||||
39
example/go2/high_level/go2_utlidar_switch.py
Normal file
39
example/go2/high_level/go2_utlidar_switch.py
Normal file
@ -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")
|
||||
|
||||
|
||||
|
||||
176
example/go2/low_level/go2_stand_example.py
Normal file
176
example/go2/low_level/go2_stand_example.py
Normal file
@ -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)
|
||||
20
example/go2/low_level/unitree_legged_const.py
Normal file
20
example/go2/low_level/unitree_legged_const.py
Normal file
@ -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
|
||||
99
example/go2w/high_level/go2w_sport_client.py
Normal file
99
example/go2w/high_level/go2w_sport_client.py
Normal file
@ -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)
|
||||
196
example/go2w/low_level/go2w_stand_example.py
Normal file
196
example/go2w/low_level/go2w_stand_example.py
Normal file
@ -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)
|
||||
24
example/go2w/low_level/unitree_legged_const.py
Normal file
24
example/go2w/low_level/unitree_legged_const.py
Normal file
@ -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
|
||||
96
example/h1/high_level/h1_loco_client_example.py
Normal file
96
example/h1/high_level/h1_loco_client_example.py
Normal file
@ -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)
|
||||
167
example/h1/low_level/h1_low_level_example.py
Normal file
167
example/h1/low_level/h1_low_level_example.py
Normal file
@ -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)
|
||||
5
example/h1/low_level/unitree_legged_const.py
Normal file
5
example/h1/low_level/unitree_legged_const.py
Normal file
@ -0,0 +1,5 @@
|
||||
HIGHLEVEL = 0xEE
|
||||
LOWLEVEL = 0xFF
|
||||
TRIGERLEVEL = 0xF0
|
||||
PosStopF = 2.146e9
|
||||
VelStopF = 16000.0
|
||||
201
example/h1_2/low_level/h1_2_low_level_example.py
Normal file
201
example/h1_2/low_level/h1_2_low_level_example.py
Normal file
@ -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)
|
||||
28
example/helloworld/publisher.py
Normal file
28
example/helloworld/publisher.py
Normal file
@ -0,0 +1,28 @@
|
||||
import time
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
|
||||
from user_data import *
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ChannelFactoryInitialize()
|
||||
|
||||
# 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()
|
||||
20
example/helloworld/subscriber.py
Normal file
20
example/helloworld/subscriber.py
Normal file
@ -0,0 +1,20 @@
|
||||
import time
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from user_data import *
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ChannelFactoryInitialize()
|
||||
# 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()
|
||||
9
example/helloworld/user_data.py
Normal file
9
example/helloworld/user_data.py
Normal file
@ -0,0 +1,9 @@
|
||||
from dataclasses import dataclass
|
||||
from cyclonedds.idl import IdlStruct
|
||||
|
||||
|
||||
# This class defines user data consisting of a float data and a string data
|
||||
@dataclass
|
||||
class UserData(IdlStruct, typename="UserData"):
|
||||
string_data: str
|
||||
float_data: float
|
||||
36
example/motionSwitcher/motion_switcher_example.py
Normal file
36
example/motionSwitcher/motion_switcher_example.py
Normal file
@ -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)
|
||||
|
||||
35
example/obstacles_avoid/obstacles_avoid_move.py
Normal file
35
example/obstacles_avoid/obstacles_avoid_move.py
Normal file
@ -0,0 +1,35 @@
|
||||
import time
|
||||
import sys
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
|
||||
from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ObstaclesAvoidClient
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
try:
|
||||
client = ObstaclesAvoidClient()
|
||||
client.SetTimeout(3.0)
|
||||
client.Init()
|
||||
|
||||
while not client.SwitchGet()[1]:
|
||||
client.SwitchSet(True)
|
||||
time.sleep(0.1)
|
||||
|
||||
print("obstacles avoid switch on")
|
||||
|
||||
client.UseRemoteCommandFromApi(True)
|
||||
time.sleep(0.5)
|
||||
client.Move(0.5, 0.0, 0.0)
|
||||
time.sleep(1.0) # move 1s
|
||||
client.Move(0.0, 0.0, 0.0)
|
||||
client.UseRemoteCommandFromApi(False)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
client.Move(0.0, 0.0, 0.0)
|
||||
client.UseRemoteCommandFromApi(False)
|
||||
print("exit!!")
|
||||
|
||||
94
example/obstacles_avoid/obstacles_avoid_switch.py
Normal file
94
example/obstacles_avoid/obstacles_avoid_switch.py
Normal file
@ -0,0 +1,94 @@
|
||||
import time
|
||||
import sys
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
|
||||
from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ObstaclesAvoidClient
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
client = ObstaclesAvoidClient()
|
||||
client.SetTimeout(3.0)
|
||||
client.Init()
|
||||
|
||||
while True:
|
||||
print("##################GetServerApiVersion###################")
|
||||
code, serverAPiVersion = client.GetServerApiVersion()
|
||||
if code != 0:
|
||||
print("get server api error. code:", code)
|
||||
else:
|
||||
print("get server api version:", serverAPiVersion)
|
||||
|
||||
if serverAPiVersion != client.GetApiVersion():
|
||||
print("api version not equal.")
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
print("##################SwitchGet###################")
|
||||
code, enable = client.SwitchGet()
|
||||
if code != 0:
|
||||
print("switch get error. code:", code)
|
||||
else:
|
||||
print("switch get success. enable:", enable)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
print("##################SwitchSet (on)###################")
|
||||
code = client.SwitchSet(True)
|
||||
if code != 0:
|
||||
print("switch set error. code:", code)
|
||||
else:
|
||||
print("switch set success.")
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
print("##################SwitchGet###################")
|
||||
code, enable1 = client.SwitchGet()
|
||||
if code != 0:
|
||||
print("switch get error. code:", code)
|
||||
else:
|
||||
print("switch get success. enable:", enable1)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
print("##################SwitchSet (off)###################")
|
||||
code = client.SwitchSet(False)
|
||||
if code != 0:
|
||||
print("switch set error. code:", code)
|
||||
else:
|
||||
print("switch set success.")
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
print("##################SwitchGet###################")
|
||||
code, enable1 = client.SwitchGet()
|
||||
if code != 0:
|
||||
print("switch get error. code:", code)
|
||||
else:
|
||||
print("switch get success. enable:", enable1)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
|
||||
print("##################SwitchSet (enable)###################")
|
||||
|
||||
code = client.SwitchSet(enable)
|
||||
if code != 0:
|
||||
print("switch set error. code:", code)
|
||||
else:
|
||||
print("switch set success. enable:", enable)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
print("##################SwitchGet###################")
|
||||
code, enable = client.SwitchGet()
|
||||
if code != 0:
|
||||
print("switch get error. code:", code)
|
||||
else:
|
||||
print("switch get success. enable:", enable)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
77
example/vui_client/vui_client_example.py
Normal file
77
example/vui_client/vui_client_example.py
Normal file
@ -0,0 +1,77 @@
|
||||
import time
|
||||
import sys
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
|
||||
from unitree_sdk2py.go2.vui.vui_client import VuiClient
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
client = VuiClient()
|
||||
client.SetTimeout(3.0)
|
||||
client.Init()
|
||||
|
||||
for i in range(1, 11):
|
||||
print("#################GetBrightness####################")
|
||||
code, level = client.GetBrightness()
|
||||
|
||||
if code != 0:
|
||||
print("get brightness error. code:", code)
|
||||
else:
|
||||
print("get brightness success. level:", level)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print("#################SetBrightness####################")
|
||||
|
||||
code = client.SetBrightness(i)
|
||||
|
||||
if code != 0:
|
||||
print("set brightness error. code:", code)
|
||||
else:
|
||||
print("set brightness success. level:", i)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print("#################SetBrightness 0####################")
|
||||
|
||||
code = client.SetBrightness(0)
|
||||
|
||||
if code != 0:
|
||||
print("set brightness error. code:", code)
|
||||
else:
|
||||
print("set brightness 0 success.")
|
||||
|
||||
for i in range(1, 11):
|
||||
print("#################GetVolume####################")
|
||||
code, level = client.GetVolume()
|
||||
|
||||
if code != 0:
|
||||
print("get volume error. code:", code)
|
||||
else:
|
||||
print("get volume success. level:", level)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print("#################SetVolume####################")
|
||||
|
||||
code = client.SetVolume(i)
|
||||
|
||||
if code != 0:
|
||||
print("set volume error. code:", code)
|
||||
else:
|
||||
print("set volume success. level:", i)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print("#################SetVolume 0####################")
|
||||
|
||||
code = client.SetVolume(0)
|
||||
|
||||
if code != 0:
|
||||
print("set volume error. code:", code)
|
||||
else:
|
||||
print("set volume 0 success.")
|
||||
131
example/wireless_controller/wireless_controller.py
Normal file
131
example/wireless_controller/wireless_controller.py
Normal file
@ -0,0 +1,131 @@
|
||||
import time
|
||||
import sys
|
||||
import struct
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
|
||||
# Uncomment the following two lines when using Go2、Go2-W、B2、B2-W、H1 robot
|
||||
# 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]
|
||||
|
||||
|
||||
def parse(self,remoteData):
|
||||
self.parse_key(remoteData)
|
||||
self.parse_botton(remoteData[2],remoteData[3])
|
||||
|
||||
print("debug unitreeRemoteController: ")
|
||||
print("Lx:", self.Lx)
|
||||
print("Rx:", self.Rx)
|
||||
print("Ry:", self.Ry)
|
||||
print("Ly:", self.Ly)
|
||||
|
||||
print("L1:", self.L1)
|
||||
print("L2:", self.L2)
|
||||
print("R1:", self.R1)
|
||||
print("R2:", self.R2)
|
||||
print("A:", self.A)
|
||||
print("B:", self.B)
|
||||
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)
|
||||
|
||||
|
||||
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()
|
||||
|
||||
while True:
|
||||
time.sleep(1)
|
||||
3
pyproject.toml
Normal file
3
pyproject.toml
Normal file
@ -0,0 +1,3 @@
|
||||
[build-system]
|
||||
requires = ["setuptools", "wheel"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
21
setup.py
Normal file
21
setup.py
Normal file
@ -0,0 +1,21 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
setup(name='unitree_sdk2py',
|
||||
version='1.0.1',
|
||||
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",
|
||||
],
|
||||
)
|
||||
10
unitree_sdk2py/__init__.py
Normal file
10
unitree_sdk2py/__init__.py
Normal file
@ -0,0 +1,10 @@
|
||||
from . import idl, utils, core, rpc, go2, b2
|
||||
|
||||
__all__ = [
|
||||
"idl"
|
||||
"utils"
|
||||
"core",
|
||||
"rpc",
|
||||
"go2",
|
||||
"b2",
|
||||
]
|
||||
16
unitree_sdk2py/b2/back_video/back_video_api.py
Normal file
16
unitree_sdk2py/b2/back_video/back_video_api.py
Normal file
@ -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
|
||||
23
unitree_sdk2py/b2/back_video/back_video_client.py
Normal file
23
unitree_sdk2py/b2/back_video/back_video_client.py
Normal file
@ -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, [])
|
||||
16
unitree_sdk2py/b2/front_video/front_video_api.py
Normal file
16
unitree_sdk2py/b2/front_video/front_video_api.py
Normal file
@ -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
|
||||
23
unitree_sdk2py/b2/front_video/front_video_client.py
Normal file
23
unitree_sdk2py/b2/front_video/front_video_client.py
Normal file
@ -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, [])
|
||||
25
unitree_sdk2py/b2/robot_state/robot_state_api.py
Normal file
25
unitree_sdk2py/b2/robot_state/robot_state_api.py
Normal file
@ -0,0 +1,25 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
ROBOT_STATE_SERVICE_NAME = "robot_state"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
ROBOT_STATE_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
ROBOT_STATE_API_ID_SERVICE_SWITCH = 1001
|
||||
ROBOT_STATE_API_ID_REPORT_FREQ = 1002
|
||||
ROBOT_STATE_API_ID_SERVICE_LIST = 1003
|
||||
|
||||
|
||||
"""
|
||||
" error code
|
||||
"""
|
||||
ROBOT_STATE_ERR_SERVICE_SWITCH = 5201
|
||||
ROBOT_STATE_ERR_SERVICE_PROTECTED = 5202
|
||||
84
unitree_sdk2py/b2/robot_state/robot_state_client.py
Normal file
84
unitree_sdk2py/b2/robot_state/robot_state_client.py
Normal file
@ -0,0 +1,84 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from ...rpc.client_internal import *
|
||||
from .robot_state_api import *
|
||||
|
||||
|
||||
"""
|
||||
" class ServiceState
|
||||
"""
|
||||
class ServiceState:
|
||||
def __init__(self, name: str = None, status: int = None, protect: bool = None):
|
||||
self.name = name
|
||||
self.status = status
|
||||
self.protect = protect
|
||||
|
||||
"""
|
||||
" class RobotStateClient
|
||||
"""
|
||||
class RobotStateClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(ROBOT_STATE_SERVICE_NAME, False)
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(ROBOT_STATE_API_VERSION)
|
||||
# regist api
|
||||
self._RegistApi(ROBOT_STATE_API_ID_SERVICE_SWITCH, 0)
|
||||
self._RegistApi(ROBOT_STATE_API_ID_REPORT_FREQ, 0)
|
||||
self._RegistApi(ROBOT_STATE_API_ID_SERVICE_LIST, 0)
|
||||
|
||||
def ServiceList(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_LIST, parameter)
|
||||
|
||||
if code != 0:
|
||||
return code, None
|
||||
|
||||
lst = []
|
||||
|
||||
d = json.loads(data)
|
||||
for t in d:
|
||||
s = ServiceState()
|
||||
s.name = t["name"]
|
||||
s.status = t["status"]
|
||||
s.protect = t["protect"]
|
||||
lst.append(s)
|
||||
|
||||
return code, lst
|
||||
|
||||
|
||||
def ServiceSwitch(self, name: str, switch: bool):
|
||||
p = {}
|
||||
p["name"] = name
|
||||
p["switch"] = int(switch)
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_SWITCH, parameter)
|
||||
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
d = json.loads(data)
|
||||
|
||||
status = d["status"]
|
||||
|
||||
if status == 5:
|
||||
return ROBOT_STATE_ERR_SERVICE_PROTECTED
|
||||
|
||||
if status != 0 and status != 1:
|
||||
return ROBOT_STATE_ERR_SERVICE_SWITCH
|
||||
|
||||
return code
|
||||
|
||||
def SetReportFreq(self, interval: int, duration: int):
|
||||
p = {}
|
||||
p["interval"] = interval
|
||||
p["duration"] = duration
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(ROBOT_STATE_API_ID_REPORT_FREQ, p)
|
||||
return code
|
||||
58
unitree_sdk2py/b2/sport/sport_api.py
Normal file
58
unitree_sdk2py/b2/sport/sport_api.py
Normal file
@ -0,0 +1,58 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
SPORT_SERVICE_NAME = "sport"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
SPORT_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
SPORT_API_ID_DAMP = 1001;
|
||||
SPORT_API_ID_BALANCESTAND = 1002;
|
||||
SPORT_API_ID_STOPMOVE = 1003;
|
||||
SPORT_API_ID_STANDUP = 1004;
|
||||
SPORT_API_ID_STANDDOWN = 1005;
|
||||
SPORT_API_ID_RECOVERYSTAND = 1006;
|
||||
SPORT_API_ID_EULER = 1007;
|
||||
SPORT_API_ID_MOVE = 1008;
|
||||
SPORT_API_ID_SIT = 1009;
|
||||
SPORT_API_ID_RISESIT = 1010;
|
||||
SPORT_API_ID_SWITCHGAIT = 1011;
|
||||
SPORT_API_ID_TRIGGER = 1012;
|
||||
SPORT_API_ID_BODYHEIGHT = 1013;
|
||||
SPORT_API_ID_FOOTRAISEHEIGHT = 1014;
|
||||
SPORT_API_ID_SPEEDLEVEL = 1015;
|
||||
SPORT_API_ID_HELLO = 1016;
|
||||
SPORT_API_ID_STRETCH = 1017;
|
||||
SPORT_API_ID_TRAJECTORYFOLLOW = 1018;
|
||||
SPORT_API_ID_CONTINUOUSGAIT = 1019;
|
||||
SPORT_API_ID_CONTENT = 1020;
|
||||
SPORT_API_ID_WALLOW = 1021;
|
||||
SPORT_API_ID_DANCE1 = 1022;
|
||||
SPORT_API_ID_DANCE2 = 1023;
|
||||
SPORT_API_ID_GETBODYHEIGHT = 1024;
|
||||
SPORT_API_ID_GETFOOTRAISEHEIGHT = 1025;
|
||||
SPORT_API_ID_GETSPEEDLEVEL = 1026;
|
||||
SPORT_API_ID_SWITCHJOYSTICK = 1027;
|
||||
SPORT_API_ID_POSE = 1028;
|
||||
SPORT_API_ID_FRONTJUMP = 1031;
|
||||
SPORT_API_ID_ECONOMICGAIT = 1035;
|
||||
SPORT_API_ID_MOVETOPOS = 1036;
|
||||
SPORT_API_ID_SWITCHEULERMODE = 1037;
|
||||
SPORT_API_ID_SWITCHMOVEMODE = 1038;
|
||||
|
||||
|
||||
"""
|
||||
" error code
|
||||
"""
|
||||
# client side
|
||||
SPORT_ERR_CLIENT_POINT_PATH = 4101
|
||||
# server side
|
||||
SPORT_ERR_SERVER_OVERTIME = 4201
|
||||
SPORT_ERR_SERVER_NOT_INIT = 4202
|
||||
241
unitree_sdk2py/b2/sport/sport_client.py
Normal file
241
unitree_sdk2py/b2/sport/sport_client.py
Normal file
@ -0,0 +1,241 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .sport_api import *
|
||||
|
||||
"""
|
||||
" SPORT_PATH_POINT_SIZE
|
||||
"""
|
||||
SPORT_PATH_POINT_SIZE = 30
|
||||
|
||||
|
||||
"""
|
||||
" class PathPoint
|
||||
"""
|
||||
class PathPoint:
|
||||
def __init__(self, timeFromStart: float, x: float, y: float, yaw: float, vx: float, vy: float, vyaw: float):
|
||||
self.timeFromStart = timeFromStart
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.yaw = yaw
|
||||
self.vx = vx
|
||||
self.vy = vy
|
||||
self.vyaw = vyaw
|
||||
|
||||
|
||||
"""
|
||||
" class SportClient
|
||||
"""
|
||||
class SportClient(Client):
|
||||
def __init__(self, enableLease: bool = False):
|
||||
super().__init__(SPORT_SERVICE_NAME, enableLease)
|
||||
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(SPORT_API_VERSION)
|
||||
|
||||
# regist api
|
||||
self._RegistApi(SPORT_API_ID_DAMP, 0)
|
||||
self._RegistApi(SPORT_API_ID_BALANCESTAND, 0)
|
||||
self._RegistApi(SPORT_API_ID_STOPMOVE, 0)
|
||||
self._RegistApi(SPORT_API_ID_STANDUP, 0)
|
||||
self._RegistApi(SPORT_API_ID_STANDDOWN, 0)
|
||||
self._RegistApi(SPORT_API_ID_RECOVERYSTAND, 0)
|
||||
self._RegistApi(SPORT_API_ID_EULER, 0)
|
||||
self._RegistApi(SPORT_API_ID_MOVE, 0)
|
||||
self._RegistApi(SPORT_API_ID_SIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_SWITCHGAIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_BODYHEIGHT, 0)
|
||||
self._RegistApi(SPORT_API_ID_FOOTRAISEHEIGHT, 0)
|
||||
self._RegistApi(SPORT_API_ID_SPEEDLEVEL, 0)
|
||||
self._RegistApi(SPORT_API_ID_TRAJECTORYFOLLOW, 0)
|
||||
self._RegistApi(SPORT_API_ID_CONTINUOUSGAIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_MOVETOPOS, 0)
|
||||
self._RegistApi(SPORT_API_ID_FRONTJUMP, 0)
|
||||
self._RegistApi(SPORT_API_ID_ECONOMICGAIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_POSE, 0)
|
||||
self._RegistApi(SPORT_API_ID_SWITCHEULERMODE, 0)
|
||||
self._RegistApi(SPORT_API_ID_SWITCHMOVEMODE, 0)
|
||||
|
||||
# 1001
|
||||
def Damp(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_DAMP, parameter)
|
||||
return code
|
||||
|
||||
# 1002
|
||||
def BalanceStand(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_BALANCESTAND, parameter)
|
||||
return code
|
||||
|
||||
# 1003
|
||||
def StopMove(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STOPMOVE, parameter)
|
||||
return code
|
||||
|
||||
# 1004
|
||||
def StandUp(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STANDUP, parameter)
|
||||
return code
|
||||
|
||||
# 1005
|
||||
def StandDown(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STANDDOWN, parameter)
|
||||
return code
|
||||
|
||||
# 1006
|
||||
def RecoveryStand(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_RECOVERYSTAND, parameter)
|
||||
return code
|
||||
|
||||
# 1007
|
||||
def Euler(self, roll: float, pitch: float, yaw: float):
|
||||
p = {}
|
||||
p["x"] = roll
|
||||
p["y"] = pitch
|
||||
p["z"] = yaw
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_EULER, parameter)
|
||||
return code
|
||||
|
||||
# 1008
|
||||
def Move(self, vx: float, vy: float, vyaw: float):
|
||||
p = {}
|
||||
p["x"] = vx
|
||||
p["y"] = vy
|
||||
p["z"] = vyaw
|
||||
parameter = json.dumps(p)
|
||||
code = self._CallNoReply(SPORT_API_ID_MOVE, parameter)
|
||||
return code
|
||||
|
||||
# 1009
|
||||
def Sit(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SIT, parameter)
|
||||
return code
|
||||
|
||||
# 1011
|
||||
def SwitchGait(self, t: int):
|
||||
p = {}
|
||||
p["data"] = t
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SWITCHGAIT, parameter)
|
||||
return code
|
||||
|
||||
# 1013
|
||||
def BodyHeight(self, height: float):
|
||||
p = {}
|
||||
p["data"] = height
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_BODYHEIGHT, parameter)
|
||||
return code
|
||||
|
||||
# 1014
|
||||
def FootRaiseHeight(self, height: float):
|
||||
p = {}
|
||||
p["data"] = height
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_FOOTRAISEHEIGHT, parameter)
|
||||
return code
|
||||
|
||||
# 1015
|
||||
def SpeedLevel(self, level: int):
|
||||
p = {}
|
||||
p["data"] = level
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SPEEDLEVEL, parameter)
|
||||
return code
|
||||
|
||||
# 1018
|
||||
def TrajectoryFollow(self, path: list):
|
||||
l = len(path)
|
||||
if l != SPORT_PATH_POINT_SIZE:
|
||||
return SPORT_ERR_CLIENT_POINT_PATH
|
||||
|
||||
path_p = []
|
||||
for i in range(l):
|
||||
point = path[i]
|
||||
p = {}
|
||||
p["t_from_start"] = point.timeFromStart
|
||||
p["x"] = point.x
|
||||
p["y"] = point.y
|
||||
p["yaw"] = point.yaw
|
||||
p["vx"] = point.vx
|
||||
p["vy"] = point.vy
|
||||
p["vyaw"] = point.vyaw
|
||||
path_p.append(p)
|
||||
|
||||
parameter = json.dumps(path_p)
|
||||
code = self._CallNoReply(SPORT_API_ID_TRAJECTORYFOLLOW, parameter)
|
||||
return code
|
||||
|
||||
# 1019
|
||||
def ContinuousGait(self, flag: int):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_CONTINUOUSGAIT, parameter)
|
||||
return code
|
||||
|
||||
# 1036
|
||||
def MoveToPos(self, x: float, y: float, yaw: float):
|
||||
p = {}
|
||||
p["x"] = x
|
||||
p["y"] = y
|
||||
p["yaw"] = yaw
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_MOVETOPOS, parameter)
|
||||
return code
|
||||
|
||||
# 1031
|
||||
def FrontJump(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_FRONTJUMP, parameter)
|
||||
return code
|
||||
|
||||
|
||||
# 1035
|
||||
def EconomicGait(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_ECONOMICGAIT, parameter)
|
||||
return code
|
||||
|
||||
# 1028
|
||||
def Pose(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_POSE, parameter)
|
||||
return code
|
||||
|
||||
# 1037
|
||||
def SwitchEulerMode(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SWITCHEULERMODE, parameter)
|
||||
return code
|
||||
|
||||
# 1038
|
||||
def SwitchMoveMode(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SWITCHMOVEMODE, parameter)
|
||||
return code
|
||||
21
unitree_sdk2py/b2/vui/vui_api.py
Normal file
21
unitree_sdk2py/b2/vui/vui_api.py
Normal file
@ -0,0 +1,21 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
VUI_SERVICE_NAME = "vui"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
VUI_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
VUI_API_ID_SETSWITCH = 1001
|
||||
VUI_API_ID_GETSWITCH = 1002
|
||||
VUI_API_ID_SETVOLUME = 1003
|
||||
VUI_API_ID_GETVOLUME = 1004
|
||||
VUI_API_ID_SETBRIGHTNESS = 1005
|
||||
VUI_API_ID_GETBRIGHTNESS = 1006
|
||||
86
unitree_sdk2py/b2/vui/vui_client.py
Normal file
86
unitree_sdk2py/b2/vui/vui_client.py
Normal file
@ -0,0 +1,86 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .vui_api import *
|
||||
|
||||
|
||||
"""
|
||||
" class VideoClient
|
||||
"""
|
||||
class VuiClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(VUI_SERVICE_NAME, False)
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(VUI_API_VERSION)
|
||||
# regist api
|
||||
self._RegistApi(VUI_API_ID_SETSWITCH, 0)
|
||||
self._RegistApi(VUI_API_ID_GETSWITCH, 0)
|
||||
self._RegistApi(VUI_API_ID_SETVOLUME, 0)
|
||||
self._RegistApi(VUI_API_ID_GETVOLUME, 0)
|
||||
self._RegistApi(VUI_API_ID_SETBRIGHTNESS, 0)
|
||||
self._RegistApi(VUI_API_ID_GETBRIGHTNESS, 0)
|
||||
|
||||
# 1001
|
||||
def SetSwitch(self, enable: int):
|
||||
p = {}
|
||||
p["enable"] = enable
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_SETSWITCH, parameter)
|
||||
return code
|
||||
|
||||
# 1002
|
||||
def GetSwitch(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_GETSWITCH, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["enable"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1003
|
||||
def SetVolume(self, level: int):
|
||||
p = {}
|
||||
p["volume"] = level
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_SETVOLUME, parameter)
|
||||
return code
|
||||
|
||||
# 1006
|
||||
def GetVolume(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_GETVOLUME, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["volume"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1005
|
||||
def SetBrightness(self, level: int):
|
||||
p = {}
|
||||
p["brightness"] = level
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_SETBRIGHTNESS, parameter)
|
||||
return code
|
||||
|
||||
# 1006
|
||||
def GetBrightness(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_GETBRIGHTNESS, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["brightness"]
|
||||
else:
|
||||
return code, None
|
||||
0
unitree_sdk2py/comm/motion_switcher/__init__.py
Normal file
0
unitree_sdk2py/comm/motion_switcher/__init__.py
Normal file
29
unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py
Normal file
29
unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py
Normal file
@ -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
|
||||
@ -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
|
||||
|
||||
0
unitree_sdk2py/core/__init__.py
Normal file
0
unitree_sdk2py/core/__init__.py
Normal file
290
unitree_sdk2py/core/channel.py
Normal file
290
unitree_sdk2py/core/channel.py
Normal file
@ -0,0 +1,290 @@
|
||||
import time
|
||||
from typing import Any, Callable
|
||||
from threading import Thread, Event
|
||||
|
||||
from cyclonedds.domain import Domain, DomainParticipant
|
||||
from cyclonedds.internal import dds_c_t
|
||||
from cyclonedds.pub import DataWriter
|
||||
from cyclonedds.sub import DataReader
|
||||
from cyclonedds.topic import Topic
|
||||
from cyclonedds.qos import Qos
|
||||
from cyclonedds.core import DDSException, Listener
|
||||
from cyclonedds.util import duration
|
||||
from cyclonedds.internal import dds_c_t, InvalidSample
|
||||
|
||||
# for channel config
|
||||
from .channel_config import ChannelConfigAutoDetermine, ChannelConfigHasInterface
|
||||
|
||||
# for singleton
|
||||
from ..utils.singleton import Singleton
|
||||
from ..utils.bqueue import BQueue
|
||||
|
||||
|
||||
"""
|
||||
" class ChannelReader
|
||||
"""
|
||||
|
||||
"""
|
||||
" class Channel
|
||||
"""
|
||||
class Channel:
|
||||
|
||||
"""
|
||||
" internal class __Reader
|
||||
"""
|
||||
class __Reader:
|
||||
def __init__(self):
|
||||
self.__reader = None
|
||||
self.__handler = None
|
||||
self.__queue = None
|
||||
self.__queueEnable = False
|
||||
self.__threadEvent = None
|
||||
self.__threadReader = None
|
||||
|
||||
def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None, handler: Callable = None, queueLen: int = 0):
|
||||
if handler is None:
|
||||
self.__reader = DataReader(participant, topic, qos)
|
||||
else:
|
||||
self.__handler = handler
|
||||
if queueLen > 0:
|
||||
self.__queueEnable = True
|
||||
self.__queue = BQueue(queueLen)
|
||||
self.__threadEvent = Event()
|
||||
self.__threadReader = Thread(target=self.__ChannelReaderThreadFunc, name="ch_reader", daemon=True)
|
||||
self.__threadReader.start()
|
||||
self.__reader = DataReader(participant, topic, qos, Listener(on_data_available=self.__OnDataAvailable))
|
||||
|
||||
def Read(self, timeout: float = None):
|
||||
sample = None
|
||||
try:
|
||||
if timeout is None:
|
||||
sample = self.__reader.take_one()
|
||||
else:
|
||||
sample = self.__reader.take_one(timeout=duration(seconds=timeout))
|
||||
except DDSException as e:
|
||||
print("[Reader] catch DDSException msg:", e.msg)
|
||||
except TimeoutError as e:
|
||||
print("[Reader] take sample timeout")
|
||||
except:
|
||||
print("[Reader] take sample error")
|
||||
|
||||
return sample
|
||||
|
||||
def Close(self):
|
||||
if self.__reader is not None:
|
||||
del self.__reader
|
||||
|
||||
if self.__queueEnable:
|
||||
self.__threadEvent.set()
|
||||
self.__queue.Interrupt()
|
||||
self.__queue.Clear()
|
||||
self.__threadReader.join()
|
||||
|
||||
def __OnDataAvailable(self, reader: DataReader):
|
||||
samples = []
|
||||
try:
|
||||
samples = reader.take(1)
|
||||
except DDSException as e:
|
||||
print("[Reader] catch DDSException error. msg:", e.msg)
|
||||
return
|
||||
except TimeoutError as e:
|
||||
print("[Reader] take sample timeout")
|
||||
return
|
||||
except:
|
||||
print("[Reader] take sample error")
|
||||
return
|
||||
|
||||
if samples is None:
|
||||
return
|
||||
|
||||
# check invalid sample
|
||||
sample = samples[0]
|
||||
if isinstance(sample, InvalidSample):
|
||||
return
|
||||
|
||||
# do sample
|
||||
if self.__queueEnable:
|
||||
self.__queue.Put(sample)
|
||||
else:
|
||||
self.__handler(sample)
|
||||
|
||||
def __ChannelReaderThreadFunc(self):
|
||||
while not self.__threadEvent.is_set():
|
||||
sample = self.__queue.Get()
|
||||
if sample is not None:
|
||||
self.__handler(sample)
|
||||
|
||||
"""
|
||||
" internal class __Writer
|
||||
"""
|
||||
class __Writer:
|
||||
def __init__(self):
|
||||
self.__writer = None
|
||||
self.__publication_matched_count = 0
|
||||
|
||||
def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None):
|
||||
self.__writer = DataWriter(participant, topic, qos, Listener(on_publication_matched=self.__OnPublicationMatched))
|
||||
time.sleep(0.2)
|
||||
|
||||
def Write(self, sample: Any, timeout: float = None):
|
||||
waitsec = 0.0 if timeout is None else timeout
|
||||
|
||||
# check publication_matched_count
|
||||
while waitsec > 0.0 and self.__publication_matched_count == 0:
|
||||
time.sleep(0.1)
|
||||
waitsec = waitsec - 0.1
|
||||
# print(time.time())
|
||||
|
||||
# check waitsec
|
||||
if timeout is not None and waitsec <= 0.0:
|
||||
return False
|
||||
|
||||
try:
|
||||
self.__writer.write(sample)
|
||||
except DDSException as e:
|
||||
print("[Writer] catch DDSException error. msg:", e.msg)
|
||||
return False
|
||||
except Exception as e:
|
||||
print("[Writer] write sample error. msg:", e.args())
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def Close(self):
|
||||
if self.__writer is not None:
|
||||
del self.__writer
|
||||
|
||||
def __OnPublicationMatched(self, writer: DataWriter, status: dds_c_t.publication_matched_status):
|
||||
self.__publication_matched_count = status.current_count
|
||||
|
||||
|
||||
# channel __init__
|
||||
def __init__(self, participant: DomainParticipant, name: str, type: Any, qos: Qos = None):
|
||||
self.__reader = self.__Reader()
|
||||
self.__writer = self.__Writer()
|
||||
self.__participant = participant
|
||||
self.__topic = Topic(self.__participant, name, type, qos)
|
||||
|
||||
def SetWriter(self, qos: Qos = None):
|
||||
self.__writer.Init(self.__participant, self.__topic, qos)
|
||||
|
||||
def SetReader(self, qos: Qos = None, handler: Callable = None, queueLen: int = 0):
|
||||
self.__reader.Init(self.__participant, self.__topic, qos, handler, queueLen)
|
||||
|
||||
def Write(self, sample: Any, timeout: float = None):
|
||||
return self.__writer.Write(sample, timeout)
|
||||
|
||||
def Read(self, timeout: float = None):
|
||||
return self.__reader.Read(timeout)
|
||||
|
||||
def CloseReader(self):
|
||||
self.__reader.Close()
|
||||
|
||||
def CloseWriter(self):
|
||||
self.__writer.Close()
|
||||
|
||||
|
||||
"""
|
||||
" class ChannelFactory
|
||||
"""
|
||||
class ChannelFactory(Singleton):
|
||||
__domain = None
|
||||
__participant = None
|
||||
__qos = None
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
def Init(self, id: int, networkInterface: str = None, qos: Qos = None):
|
||||
config = None
|
||||
# choose config
|
||||
if networkInterface is None:
|
||||
config = ChannelConfigAutoDetermine
|
||||
else:
|
||||
config = ChannelConfigHasInterface.replace('$__IF_NAME__$', networkInterface)
|
||||
|
||||
try:
|
||||
self.__domain = Domain(id, config)
|
||||
except DDSException as e:
|
||||
print("[ChannelFactory] create domain error. msg:", e.msg)
|
||||
return False
|
||||
except:
|
||||
print("[ChannelFactory] create domain error.")
|
||||
return False
|
||||
|
||||
try:
|
||||
self.__participant = DomainParticipant(id)
|
||||
except DDSException as e:
|
||||
print("[ChannelFactory] create domain participant error. msg:", e.msg)
|
||||
return False
|
||||
except:
|
||||
print("[ChannelFactory] create domain participant error")
|
||||
return False
|
||||
|
||||
self.__qos = qos
|
||||
|
||||
return True
|
||||
|
||||
def CreateChannel(self, name: str, type: Any):
|
||||
return Channel(self.__participant, name, type, self.__qos)
|
||||
|
||||
def CreateSendChannel(self, name: str, type: Any):
|
||||
channel = self.CreateChannel(name, type)
|
||||
channel.SetWriter(None)
|
||||
return channel
|
||||
|
||||
def CreateRecvChannel(self, name: str, type: Any, handler: Callable = None, queueLen: int = 0):
|
||||
channel = self.CreateChannel(name, type)
|
||||
channel.SetReader(None, handler, queueLen)
|
||||
return channel
|
||||
|
||||
|
||||
"""
|
||||
" class ChannelPublisher
|
||||
"""
|
||||
class ChannelPublisher:
|
||||
def __init__(self, name: str, type: Any):
|
||||
factory = ChannelFactory()
|
||||
self.__channel = factory.CreateChannel(name, type)
|
||||
self.__inited = False
|
||||
|
||||
def Init(self):
|
||||
if not self.__inited:
|
||||
self.__channel.SetWriter(None)
|
||||
self.__inited = True
|
||||
|
||||
def Close(self):
|
||||
self.__channel.CloseWriter()
|
||||
self.__inited = False
|
||||
|
||||
def Write(self, sample: Any, timeout: float = None):
|
||||
return self.__channel.Write(sample, timeout)
|
||||
|
||||
"""
|
||||
" class ChannelSubscriber
|
||||
"""
|
||||
class ChannelSubscriber:
|
||||
def __init__(self, name: str, type: Any):
|
||||
factory = ChannelFactory()
|
||||
self.__channel = factory.CreateChannel(name, type)
|
||||
self.__inited = False
|
||||
|
||||
def Init(self, handler: Callable = None, queueLen: int = 0):
|
||||
if not self.__inited:
|
||||
self.__channel.SetReader(None, handler, queueLen)
|
||||
self.__inited = True
|
||||
|
||||
def Close(self):
|
||||
self.__channel.CloseReader()
|
||||
self.__inited = False
|
||||
|
||||
def Read(self, timeout: int = None):
|
||||
return self.__channel.Read(timeout)
|
||||
|
||||
"""
|
||||
" function ChannelFactoryInitialize. used to intialize channel everenment.
|
||||
"""
|
||||
def ChannelFactoryInitialize(id: int = 0, networkInterface: str = None):
|
||||
factory = ChannelFactory()
|
||||
if not factory.Init(id, networkInterface):
|
||||
raise Exception("channel factory init error.")
|
||||
25
unitree_sdk2py/core/channel_config.py
Normal file
25
unitree_sdk2py/core/channel_config.py
Normal file
@ -0,0 +1,25 @@
|
||||
ChannelConfigHasInterface = '''<?xml version="1.0" encoding="UTF-8" ?>
|
||||
<CycloneDDS>
|
||||
<Domain Id="any">
|
||||
<General>
|
||||
<Interfaces>
|
||||
<NetworkInterface name="$__IF_NAME__$" priority="default" multicast="default"/>
|
||||
</Interfaces>
|
||||
</General>
|
||||
<Tracing>
|
||||
<Verbosity>config</Verbosity>
|
||||
<OutputFile>/tmp/cdds.LOG</OutputFile>
|
||||
</Tracing>
|
||||
</Domain>
|
||||
</CycloneDDS>'''
|
||||
|
||||
ChannelConfigAutoDetermine = '''<?xml version="1.0" encoding="UTF-8" ?>
|
||||
<CycloneDDS>
|
||||
<Domain Id="any">
|
||||
<General>
|
||||
<Interfaces>
|
||||
<NetworkInterface autodetermine=\"true\" priority=\"default\" multicast=\"default\" />
|
||||
</Interfaces>
|
||||
</General>
|
||||
</Domain>
|
||||
</CycloneDDS>'''
|
||||
34
unitree_sdk2py/core/channel_name.py
Normal file
34
unitree_sdk2py/core/channel_name.py
Normal file
@ -0,0 +1,34 @@
|
||||
from enum import Enum
|
||||
|
||||
"""
|
||||
" Enum ChannelType
|
||||
"""
|
||||
class ChannelType(Enum):
|
||||
SEND = 0
|
||||
RECV = 1
|
||||
|
||||
"""
|
||||
" function GetClientChannelName
|
||||
"""
|
||||
def GetClientChannelName(serviceName: str, channelType: ChannelType):
|
||||
name = "rt/api/" + serviceName
|
||||
|
||||
if channelType == ChannelType.SEND:
|
||||
name += "/request"
|
||||
else:
|
||||
name += "/response"
|
||||
|
||||
return name
|
||||
|
||||
"""
|
||||
" function GetClientChannelName
|
||||
"""
|
||||
def GetServerChannelName(serviceName: str, channelType: ChannelType):
|
||||
name = "rt/api/" + serviceName
|
||||
|
||||
if channelType == ChannelType.SEND:
|
||||
name += "/response"
|
||||
else:
|
||||
name += "/request"
|
||||
|
||||
return name
|
||||
19
unitree_sdk2py/g1/arm/g1_arm_action_api.py
Normal file
19
unitree_sdk2py/g1/arm/g1_arm_action_api.py
Normal file
@ -0,0 +1,19 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
ARM_ACTION_SERVICE_NAME = "arm"
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
ARM_ACTION_API_VERSION = "1.0.0.14"
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION = 7106
|
||||
ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST = 7107
|
||||
|
||||
"""
|
||||
" error code
|
||||
"""
|
||||
56
unitree_sdk2py/g1/arm/g1_arm_action_client.py
Normal file
56
unitree_sdk2py/g1/arm/g1_arm_action_client.py
Normal file
@ -0,0 +1,56 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .g1_arm_action_api import *
|
||||
|
||||
action_map = {
|
||||
"release arm": 99,
|
||||
"two-hand kiss": 11,
|
||||
"left kiss": 12,
|
||||
"right kiss": 13,
|
||||
"hands up": 15,
|
||||
"clap": 17,
|
||||
"high five": 18,
|
||||
"hug": 19,
|
||||
"heart": 20,
|
||||
"right heart": 21,
|
||||
"reject": 22,
|
||||
"right hand up": 23,
|
||||
"x-ray": 24,
|
||||
"face wave": 25,
|
||||
"high wave": 26,
|
||||
"shake hand": 27,
|
||||
}
|
||||
|
||||
|
||||
"""
|
||||
" class SportClient
|
||||
"""
|
||||
class G1ArmActionClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(ARM_ACTION_SERVICE_NAME, False)
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(ARM_ACTION_API_VERSION)
|
||||
|
||||
# regist api
|
||||
self._RegistApi(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION, 0)
|
||||
self._RegistApi(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST, 0)
|
||||
|
||||
## API Call ##
|
||||
def ExecuteAction(self, action_id: int):
|
||||
p = {}
|
||||
p["data"] = action_id
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION, parameter)
|
||||
return code
|
||||
|
||||
def GetActionList(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST, parameter)
|
||||
if code == 0:
|
||||
return code, json.loads(data)
|
||||
else:
|
||||
return code, None
|
||||
24
unitree_sdk2py/g1/audio/g1_audio_api.py
Normal file
24
unitree_sdk2py/g1/audio/g1_audio_api.py
Normal file
@ -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
|
||||
"""
|
||||
62
unitree_sdk2py/g1/audio/g1_audio_client.py
Normal file
62
unitree_sdk2py/g1/audio/g1_audio_client.py
Normal file
@ -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
|
||||
32
unitree_sdk2py/g1/loco/g1_loco_api.py
Normal file
32
unitree_sdk2py/g1/loco/g1_loco_api.py
Normal file
@ -0,0 +1,32 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
LOCO_SERVICE_NAME = "sport"
|
||||
|
||||
|
||||
"""
|
||||
" 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
|
||||
"""
|
||||
127
unitree_sdk2py/g1/loco/g1_loco_client.py
Normal file
127
unitree_sdk2py/g1/loco/g1_loco_client.py
Normal file
@ -0,0 +1,127 @@
|
||||
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 Squat2StandUp(self):
|
||||
self.SetFsmId(706)
|
||||
|
||||
def Lie2StandUp(self):
|
||||
self.SetFsmId(702)
|
||||
|
||||
def Sit(self):
|
||||
self.SetFsmId(3)
|
||||
|
||||
def StandUp2Squat(self):
|
||||
self.SetFsmId(706)
|
||||
|
||||
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)
|
||||
|
||||
0
unitree_sdk2py/go2/__init__.py
Normal file
0
unitree_sdk2py/go2/__init__.py
Normal file
0
unitree_sdk2py/go2/obstacles_avoid/__init__.py
Normal file
0
unitree_sdk2py/go2/obstacles_avoid/__init__.py
Normal file
19
unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_api.py
Normal file
19
unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_api.py
Normal file
@ -0,0 +1,19 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
OBSTACLES_AVOID_SERVICE_NAME = "obstacles_avoid"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
OBSTACLES_AVOID_API_VERSION = "1.0.0.2"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
OBSTACLES_AVOID_API_ID_SWITCH_SET = 1001
|
||||
OBSTACLES_AVOID_API_ID_SWITCH_GET = 1002
|
||||
OBSTACLES_AVOID_API_ID_MOVE = 1003
|
||||
OBSTACLES_AVOID_API_ID_USE_REMOTE_COMMAND_FROM_API = 1004
|
||||
60
unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_client.py
Normal file
60
unitree_sdk2py/go2/obstacles_avoid/obstacles_avoid_client.py
Normal file
@ -0,0 +1,60 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .obstacles_avoid_api import *
|
||||
|
||||
|
||||
"""
|
||||
" class ObstaclesAvoidClient
|
||||
"""
|
||||
class ObstaclesAvoidClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(OBSTACLES_AVOID_SERVICE_NAME, False)
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(OBSTACLES_AVOID_API_VERSION)
|
||||
# regist api
|
||||
self._RegistApi(OBSTACLES_AVOID_API_ID_SWITCH_SET, 0)
|
||||
self._RegistApi(OBSTACLES_AVOID_API_ID_SWITCH_GET, 0)
|
||||
self._RegistApi(OBSTACLES_AVOID_API_ID_MOVE, 0)
|
||||
self._RegistApi(OBSTACLES_AVOID_API_ID_USE_REMOTE_COMMAND_FROM_API, 0)
|
||||
|
||||
# 1001
|
||||
def SwitchSet(self, on: bool):
|
||||
p = {}
|
||||
p["enable"] = on
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(OBSTACLES_AVOID_API_ID_SWITCH_SET, parameter)
|
||||
return code
|
||||
|
||||
# 1002
|
||||
def SwitchGet(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(OBSTACLES_AVOID_API_ID_SWITCH_GET, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["enable"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1003
|
||||
def Move(self, vx: float, vy: float, vyaw: float):
|
||||
p = {}
|
||||
p["x"] = vx
|
||||
p["y"] = vy
|
||||
p["yaw"] = vyaw
|
||||
p["mode"] = 0
|
||||
parameter = json.dumps(p)
|
||||
code = self._CallNoReply(OBSTACLES_AVOID_API_ID_MOVE, parameter)
|
||||
return code
|
||||
|
||||
def UseRemoteCommandFromApi(self, isRemoteCommandsFromApi: bool):
|
||||
p = {}
|
||||
p["is_remote_commands_from_api"] = isRemoteCommandsFromApi
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(OBSTACLES_AVOID_API_ID_USE_REMOTE_COMMAND_FROM_API, parameter)
|
||||
return code
|
||||
0
unitree_sdk2py/go2/robot_state/__init__.py
Normal file
0
unitree_sdk2py/go2/robot_state/__init__.py
Normal file
25
unitree_sdk2py/go2/robot_state/robot_state_api.py
Normal file
25
unitree_sdk2py/go2/robot_state/robot_state_api.py
Normal file
@ -0,0 +1,25 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
ROBOT_STATE_SERVICE_NAME = "robot_state"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
ROBOT_STATE_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
ROBOT_STATE_API_ID_SERVICE_SWITCH = 1001
|
||||
ROBOT_STATE_API_ID_REPORT_FREQ = 1002
|
||||
ROBOT_STATE_API_ID_SERVICE_LIST = 1003
|
||||
|
||||
|
||||
"""
|
||||
" error code
|
||||
"""
|
||||
ROBOT_STATE_ERR_SERVICE_SWITCH = 5201
|
||||
ROBOT_STATE_ERR_SERVICE_PROTECTED = 5202
|
||||
84
unitree_sdk2py/go2/robot_state/robot_state_client.py
Normal file
84
unitree_sdk2py/go2/robot_state/robot_state_client.py
Normal file
@ -0,0 +1,84 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from ...rpc.internal import *
|
||||
from .robot_state_api import *
|
||||
|
||||
|
||||
"""
|
||||
" class ServiceState
|
||||
"""
|
||||
class ServiceState:
|
||||
def __init__(self, name: str = None, status: int = None, protect: bool = None):
|
||||
self.name = name
|
||||
self.status = status
|
||||
self.protect = protect
|
||||
|
||||
"""
|
||||
" class RobotStateClient
|
||||
"""
|
||||
class RobotStateClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(ROBOT_STATE_SERVICE_NAME, False)
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(ROBOT_STATE_API_VERSION)
|
||||
# regist api
|
||||
self._RegistApi(ROBOT_STATE_API_ID_SERVICE_SWITCH, 0)
|
||||
self._RegistApi(ROBOT_STATE_API_ID_REPORT_FREQ, 0)
|
||||
self._RegistApi(ROBOT_STATE_API_ID_SERVICE_LIST, 0)
|
||||
|
||||
def ServiceList(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_LIST, parameter)
|
||||
|
||||
if code != 0:
|
||||
return code, None
|
||||
|
||||
lst = []
|
||||
|
||||
d = json.loads(data)
|
||||
for t in d:
|
||||
s = ServiceState()
|
||||
s.name = t["name"]
|
||||
s.status = t["status"]
|
||||
s.protect = t["protect"]
|
||||
lst.append(s)
|
||||
|
||||
return code, lst
|
||||
|
||||
|
||||
def ServiceSwitch(self, name: str, switch: bool):
|
||||
p = {}
|
||||
p["name"] = name
|
||||
p["switch"] = int(switch)
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(ROBOT_STATE_API_ID_SERVICE_SWITCH, parameter)
|
||||
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
d = json.loads(data)
|
||||
|
||||
status = d["status"]
|
||||
|
||||
if status == 5:
|
||||
return ROBOT_STATE_ERR_SERVICE_PROTECTED
|
||||
|
||||
if status != 0 and status != 1:
|
||||
return ROBOT_STATE_ERR_SERVICE_SWITCH
|
||||
|
||||
return code
|
||||
|
||||
def SetReportFreq(self, interval: int, duration: int):
|
||||
p = {}
|
||||
p["interval"] = interval
|
||||
p["duration"] = duration
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(ROBOT_STATE_API_ID_REPORT_FREQ, p)
|
||||
return code
|
||||
0
unitree_sdk2py/go2/sport/__init__.py
Normal file
0
unitree_sdk2py/go2/sport/__init__.py
Normal file
74
unitree_sdk2py/go2/sport/sport_api.py
Normal file
74
unitree_sdk2py/go2/sport/sport_api.py
Normal file
@ -0,0 +1,74 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
SPORT_SERVICE_NAME = "sport"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
SPORT_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
SPORT_API_ID_DAMP = 1001
|
||||
SPORT_API_ID_BALANCESTAND = 1002
|
||||
SPORT_API_ID_STOPMOVE = 1003
|
||||
SPORT_API_ID_STANDUP = 1004
|
||||
SPORT_API_ID_STANDDOWN = 1005
|
||||
SPORT_API_ID_RECOVERYSTAND = 1006
|
||||
SPORT_API_ID_EULER = 1007
|
||||
SPORT_API_ID_MOVE = 1008
|
||||
SPORT_API_ID_SIT = 1009
|
||||
SPORT_API_ID_RISESIT = 1010
|
||||
SPORT_API_ID_SWITCHGAIT = 1011
|
||||
SPORT_API_ID_TRIGGER = 1012
|
||||
SPORT_API_ID_BODYHEIGHT = 1013
|
||||
SPORT_API_ID_FOOTRAISEHEIGHT = 1014
|
||||
SPORT_API_ID_SPEEDLEVEL = 1015
|
||||
SPORT_API_ID_HELLO = 1016
|
||||
SPORT_API_ID_STRETCH = 1017
|
||||
SPORT_API_ID_TRAJECTORYFOLLOW = 1018
|
||||
SPORT_API_ID_CONTINUOUSGAIT = 1019
|
||||
SPORT_API_ID_CONTENT = 1020
|
||||
SPORT_API_ID_WALLOW = 1021
|
||||
SPORT_API_ID_DANCE1 = 1022
|
||||
SPORT_API_ID_DANCE2 = 1023
|
||||
SPORT_API_ID_GETBODYHEIGHT = 1024
|
||||
SPORT_API_ID_GETFOOTRAISEHEIGHT = 1025
|
||||
SPORT_API_ID_GETSPEEDLEVEL = 1026
|
||||
SPORT_API_ID_SWITCHJOYSTICK = 1027
|
||||
SPORT_API_ID_POSE = 1028
|
||||
SPORT_API_ID_SCRAPE = 1029
|
||||
SPORT_API_ID_FRONTFLIP = 1030
|
||||
SPORT_API_ID_FRONTJUMP = 1031
|
||||
SPORT_API_ID_FRONTPOUNCE = 1032
|
||||
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
|
||||
"""
|
||||
# client side
|
||||
SPORT_ERR_CLIENT_POINT_PATH = 4101
|
||||
# server side
|
||||
SPORT_ERR_SERVER_OVERTIME = 4201
|
||||
SPORT_ERR_SERVER_NOT_INIT = 4202
|
||||
446
unitree_sdk2py/go2/sport/sport_client.py
Normal file
446
unitree_sdk2py/go2/sport/sport_client.py
Normal file
@ -0,0 +1,446 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .sport_api import *
|
||||
|
||||
"""
|
||||
" SPORT_PATH_POINT_SIZE
|
||||
"""
|
||||
SPORT_PATH_POINT_SIZE = 30
|
||||
|
||||
|
||||
"""
|
||||
" class PathPoint
|
||||
"""
|
||||
class PathPoint:
|
||||
def __init__(self, timeFromStart: float, x: float, y: float, yaw: float, vx: float, vy: float, vyaw: float):
|
||||
self.timeFromStart = timeFromStart
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.yaw = yaw
|
||||
self.vx = vx
|
||||
self.vy = vy
|
||||
self.vyaw = vyaw
|
||||
|
||||
|
||||
"""
|
||||
" class SportClient
|
||||
"""
|
||||
class SportClient(Client):
|
||||
def __init__(self, enableLease: bool = False):
|
||||
super().__init__(SPORT_SERVICE_NAME, enableLease)
|
||||
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(SPORT_API_VERSION)
|
||||
|
||||
# regist api
|
||||
self._RegistApi(SPORT_API_ID_DAMP, 0)
|
||||
self._RegistApi(SPORT_API_ID_BALANCESTAND, 0)
|
||||
self._RegistApi(SPORT_API_ID_STOPMOVE, 0)
|
||||
self._RegistApi(SPORT_API_ID_STANDUP, 0)
|
||||
self._RegistApi(SPORT_API_ID_STANDDOWN, 0)
|
||||
self._RegistApi(SPORT_API_ID_RECOVERYSTAND, 0)
|
||||
self._RegistApi(SPORT_API_ID_EULER, 0)
|
||||
self._RegistApi(SPORT_API_ID_MOVE, 0)
|
||||
self._RegistApi(SPORT_API_ID_SIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_RISESIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_SWITCHGAIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_TRIGGER, 0)
|
||||
self._RegistApi(SPORT_API_ID_BODYHEIGHT, 0)
|
||||
self._RegistApi(SPORT_API_ID_FOOTRAISEHEIGHT, 0)
|
||||
self._RegistApi(SPORT_API_ID_SPEEDLEVEL, 0)
|
||||
self._RegistApi(SPORT_API_ID_HELLO, 0)
|
||||
self._RegistApi(SPORT_API_ID_STRETCH, 0)
|
||||
self._RegistApi(SPORT_API_ID_TRAJECTORYFOLLOW, 0)
|
||||
self._RegistApi(SPORT_API_ID_CONTINUOUSGAIT, 0)
|
||||
# self._RegistApi(SPORT_API_ID_CONTENT, 0)
|
||||
self._RegistApi(SPORT_API_ID_WALLOW, 0)
|
||||
self._RegistApi(SPORT_API_ID_DANCE1, 0)
|
||||
self._RegistApi(SPORT_API_ID_DANCE2, 0)
|
||||
# self._RegistApi(SPORT_API_ID_GETBODYHEIGHT, 0)
|
||||
# self._RegistApi(SPORT_API_ID_GETFOOTRAISEHEIGHT, 0)
|
||||
# self._RegistApi(SPORT_API_ID_GETSPEEDLEVEL, 0)
|
||||
self._RegistApi(SPORT_API_ID_SWITCHJOYSTICK, 0)
|
||||
self._RegistApi(SPORT_API_ID_POSE, 0)
|
||||
self._RegistApi(SPORT_API_ID_SCRAPE, 0)
|
||||
self._RegistApi(SPORT_API_ID_FRONTFLIP, 0)
|
||||
self._RegistApi(SPORT_API_ID_FRONTJUMP, 0)
|
||||
self._RegistApi(SPORT_API_ID_FRONTPOUNCE, 0)
|
||||
self._RegistApi(SPORT_API_ID_WIGGLEHIPS, 0)
|
||||
self._RegistApi(SPORT_API_ID_GETSTATE, 0)
|
||||
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 = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_DAMP, parameter)
|
||||
return code
|
||||
|
||||
# 1002
|
||||
def BalanceStand(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_BALANCESTAND, parameter)
|
||||
return code
|
||||
|
||||
# 1003
|
||||
def StopMove(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STOPMOVE, parameter)
|
||||
return code
|
||||
|
||||
# 1004
|
||||
def StandUp(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STANDUP, parameter)
|
||||
return code
|
||||
|
||||
# 1005
|
||||
def StandDown(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STANDDOWN, parameter)
|
||||
return code
|
||||
|
||||
# 1006
|
||||
def RecoveryStand(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_RECOVERYSTAND, parameter)
|
||||
return code
|
||||
|
||||
# 1007
|
||||
def Euler(self, roll: float, pitch: float, yaw: float):
|
||||
p = {}
|
||||
p["x"] = roll
|
||||
p["y"] = pitch
|
||||
p["z"] = yaw
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_EULER, parameter)
|
||||
return code
|
||||
|
||||
# 1008
|
||||
def Move(self, vx: float, vy: float, vyaw: float):
|
||||
p = {}
|
||||
p["x"] = vx
|
||||
p["y"] = vy
|
||||
p["z"] = vyaw
|
||||
parameter = json.dumps(p)
|
||||
code = self._CallNoReply(SPORT_API_ID_MOVE, parameter)
|
||||
return code
|
||||
|
||||
# 1009
|
||||
def Sit(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SIT, parameter)
|
||||
return code
|
||||
|
||||
#1010
|
||||
def RiseSit(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_RISESIT, parameter)
|
||||
return code
|
||||
|
||||
# 1011
|
||||
def SwitchGait(self, t: int):
|
||||
p = {}
|
||||
p["data"] = t
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SWITCHGAIT, parameter)
|
||||
return code
|
||||
|
||||
# 1012
|
||||
def Trigger(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_TRIGGER, parameter)
|
||||
return code
|
||||
|
||||
# 1013
|
||||
def BodyHeight(self, height: float):
|
||||
p = {}
|
||||
p["data"] = height
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_BODYHEIGHT, parameter)
|
||||
return code
|
||||
|
||||
# 1014
|
||||
def FootRaiseHeight(self, height: float):
|
||||
p = {}
|
||||
p["data"] = height
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_FOOTRAISEHEIGHT, parameter)
|
||||
return code
|
||||
|
||||
# 1015
|
||||
def SpeedLevel(self, level: int):
|
||||
p = {}
|
||||
p["data"] = level
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SPEEDLEVEL, parameter)
|
||||
return code
|
||||
|
||||
# 1016
|
||||
def Hello(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_HELLO, parameter)
|
||||
return code
|
||||
|
||||
# 1017
|
||||
def Stretch(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_STRETCH, parameter)
|
||||
return code
|
||||
|
||||
# 1018
|
||||
def TrajectoryFollow(self, path: list):
|
||||
l = len(path)
|
||||
if l != SPORT_PATH_POINT_SIZE:
|
||||
return SPORT_ERR_CLIENT_POINT_PATH
|
||||
|
||||
path_p = []
|
||||
for i in range(l):
|
||||
point = path[i]
|
||||
p = {}
|
||||
p["t_from_start"] = point.timeFromStart
|
||||
p["x"] = point.x
|
||||
p["y"] = point.y
|
||||
p["yaw"] = point.yaw
|
||||
p["vx"] = point.vx
|
||||
p["vy"] = point.vy
|
||||
p["vyaw"] = point.vyaw
|
||||
path_p.append(p)
|
||||
|
||||
parameter = json.dumps(path_p)
|
||||
code = self._CallNoReply(SPORT_API_ID_TRAJECTORYFOLLOW, parameter)
|
||||
return code
|
||||
|
||||
# 1019
|
||||
def ContinuousGait(self, flag: int):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_CONTINUOUSGAIT, parameter)
|
||||
return code
|
||||
|
||||
# # 1020
|
||||
# def Content(self):
|
||||
# p = {}
|
||||
# parameter = json.dumps(p)
|
||||
# code, data = self._Call(SPORT_API_ID_CONTENT, parameter)
|
||||
# return code
|
||||
|
||||
# 1021
|
||||
def Wallow(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_WALLOW, parameter)
|
||||
return code
|
||||
|
||||
# 1022
|
||||
def Dance1(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_DANCE1, parameter)
|
||||
return code
|
||||
|
||||
# 1023
|
||||
def Dance2(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_DANCE2, parameter)
|
||||
return code
|
||||
|
||||
# 1025
|
||||
def GetFootRaiseHeight(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(SPORT_API_ID_GETFOOTRAISEHEIGHT, parameter)
|
||||
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["data"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
|
||||
# 1026
|
||||
def GetSpeedLevel(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(SPORT_API_ID_GETSPEEDLEVEL, parameter)
|
||||
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["data"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1027
|
||||
def SwitchJoystick(self, on: bool):
|
||||
p = {}
|
||||
p["data"] = on
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SWITCHJOYSTICK, parameter)
|
||||
return code
|
||||
|
||||
# 1028
|
||||
def Pose(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_POSE, parameter)
|
||||
return code
|
||||
|
||||
# 1029
|
||||
def Scrape(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_SCRAPE, parameter)
|
||||
return code
|
||||
|
||||
# 1030
|
||||
def FrontFlip(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_FRONTFLIP, parameter)
|
||||
return code
|
||||
|
||||
# 1031
|
||||
def FrontJump(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_FRONTJUMP, parameter)
|
||||
return code
|
||||
|
||||
# 1032
|
||||
def FrontPounce(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_FRONTPOUNCE, parameter)
|
||||
return code
|
||||
|
||||
# 1033
|
||||
def WiggleHips(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_WIGGLEHIPS, parameter)
|
||||
return code
|
||||
|
||||
# 1034
|
||||
def GetState(self, keys: list):
|
||||
parameter = json.dumps(keys)
|
||||
code, data = self._Call(SPORT_API_ID_GETSTATE, parameter)
|
||||
if code == 0:
|
||||
return code, json.loads(data)
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1035
|
||||
def EconomicGait(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_ECONOMICGAIT, parameter)
|
||||
return code
|
||||
|
||||
# 1036
|
||||
def Heart(self):
|
||||
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
|
||||
0
unitree_sdk2py/go2/video/__init__.py
Normal file
0
unitree_sdk2py/go2/video/__init__.py
Normal file
16
unitree_sdk2py/go2/video/video_api.py
Normal file
16
unitree_sdk2py/go2/video/video_api.py
Normal file
@ -0,0 +1,16 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
VIDEO_SERVICE_NAME = "videohub"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
VIDEO_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
VIDEO_API_ID_GETIMAGESAMPLE = 1001
|
||||
23
unitree_sdk2py/go2/video/video_client.py
Normal file
23
unitree_sdk2py/go2/video/video_client.py
Normal file
@ -0,0 +1,23 @@
|
||||
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, [])
|
||||
0
unitree_sdk2py/go2/vui/__init__.py
Normal file
0
unitree_sdk2py/go2/vui/__init__.py
Normal file
21
unitree_sdk2py/go2/vui/vui_api.py
Normal file
21
unitree_sdk2py/go2/vui/vui_api.py
Normal file
@ -0,0 +1,21 @@
|
||||
"""
|
||||
" service name
|
||||
"""
|
||||
VUI_SERVICE_NAME = "vui"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
VUI_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
VUI_API_ID_SETSWITCH = 1001
|
||||
VUI_API_ID_GETSWITCH = 1002
|
||||
VUI_API_ID_SETVOLUME = 1003
|
||||
VUI_API_ID_GETVOLUME = 1004
|
||||
VUI_API_ID_SETBRIGHTNESS = 1005
|
||||
VUI_API_ID_GETBRIGHTNESS = 1006
|
||||
86
unitree_sdk2py/go2/vui/vui_client.py
Normal file
86
unitree_sdk2py/go2/vui/vui_client.py
Normal file
@ -0,0 +1,86 @@
|
||||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .vui_api import *
|
||||
|
||||
|
||||
"""
|
||||
" class VideoClient
|
||||
"""
|
||||
class VuiClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(VUI_SERVICE_NAME, False)
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(VUI_API_VERSION)
|
||||
# regist api
|
||||
self._RegistApi(VUI_API_ID_SETSWITCH, 0)
|
||||
self._RegistApi(VUI_API_ID_GETSWITCH, 0)
|
||||
self._RegistApi(VUI_API_ID_SETVOLUME, 0)
|
||||
self._RegistApi(VUI_API_ID_GETVOLUME, 0)
|
||||
self._RegistApi(VUI_API_ID_SETBRIGHTNESS, 0)
|
||||
self._RegistApi(VUI_API_ID_GETBRIGHTNESS, 0)
|
||||
|
||||
# 1001
|
||||
def SetSwitch(self, enable: int):
|
||||
p = {}
|
||||
p["enable"] = enable
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_SETSWITCH, parameter)
|
||||
return code
|
||||
|
||||
# 1002
|
||||
def GetSwitch(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_GETSWITCH, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["enable"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1003
|
||||
def SetVolume(self, level: int):
|
||||
p = {}
|
||||
p["volume"] = level
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_SETVOLUME, parameter)
|
||||
return code
|
||||
|
||||
# 1006
|
||||
def GetVolume(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_GETVOLUME, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["volume"]
|
||||
else:
|
||||
return code, None
|
||||
|
||||
# 1005
|
||||
def SetBrightness(self, level: int):
|
||||
p = {}
|
||||
p["brightness"] = level
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_SETBRIGHTNESS, parameter)
|
||||
return code
|
||||
|
||||
# 1006
|
||||
def GetBrightness(self):
|
||||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
|
||||
code, data = self._Call(VUI_API_ID_GETBRIGHTNESS, parameter)
|
||||
if code == 0:
|
||||
d = json.loads(data)
|
||||
return code, d["brightness"]
|
||||
else:
|
||||
return code, None
|
||||
31
unitree_sdk2py/h1/loco/h1_loco_api.py
Normal file
31
unitree_sdk2py/h1/loco/h1_loco_api.py
Normal file
@ -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
|
||||
"""
|
||||
83
unitree_sdk2py/h1/loco/h1_loco_client.py
Normal file
83
unitree_sdk2py/h1/loco/h1_loco_client.py
Normal file
@ -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)
|
||||
12
unitree_sdk2py/idl/__init__.py
Normal file
12
unitree_sdk2py/idl/__init__.py
Normal file
@ -0,0 +1,12 @@
|
||||
from .default import *
|
||||
from . import builtin_interfaces, geometry_msgs, sensor_msgs, std_msgs, unitree_go, unitree_api
|
||||
|
||||
__all__ = [
|
||||
"builtin_interfaces",
|
||||
"geometry_msgs",
|
||||
"sensor_msgs",
|
||||
"std_msgs",
|
||||
"unitree_go",
|
||||
"unitree_hg",
|
||||
"unitree_api",
|
||||
]
|
||||
9
unitree_sdk2py/idl/builtin_interfaces/__init__.py
Normal file
9
unitree_sdk2py/idl/builtin_interfaces/__init__.py
Normal file
@ -0,0 +1,9 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: builtin_interfaces
|
||||
|
||||
"""
|
||||
|
||||
from . import msg
|
||||
__all__ = ["msg", ]
|
||||
9
unitree_sdk2py/idl/builtin_interfaces/msg/__init__.py
Normal file
9
unitree_sdk2py/idl/builtin_interfaces/msg/__init__.py
Normal file
@ -0,0 +1,9 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: builtin_interfaces.msg
|
||||
|
||||
"""
|
||||
|
||||
from . import dds_
|
||||
__all__ = ["dds_", ]
|
||||
28
unitree_sdk2py/idl/builtin_interfaces/msg/dds_/_Time_.py
Normal file
28
unitree_sdk2py/idl/builtin_interfaces/msg/dds_/_Time_.py
Normal file
@ -0,0 +1,28 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: builtin_interfaces.msg.dds_
|
||||
IDL file: Time_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import builtin_interfaces
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class Time_(idl.IdlStruct, typename="builtin_interfaces.msg.dds_.Time_"):
|
||||
sec: types.int32
|
||||
nanosec: types.uint32
|
||||
|
||||
|
||||
@ -0,0 +1,9 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: builtin_interfaces.msg.dds_
|
||||
|
||||
"""
|
||||
|
||||
from ._Time_ import Time_
|
||||
__all__ = ["Time_", ]
|
||||
268
unitree_sdk2py/idl/default.py
Normal file
268
unitree_sdk2py/idl/default.py
Normal file
@ -0,0 +1,268 @@
|
||||
from .builtin_interfaces.msg.dds_ import *
|
||||
from .std_msgs.msg.dds_ import *
|
||||
from .geometry_msgs.msg.dds_ import *
|
||||
from .nav_msgs.msg.dds_ import *
|
||||
from .sensor_msgs.msg.dds_ import *
|
||||
from .unitree_go.msg.dds_ import *
|
||||
from .unitree_api.msg.dds_ import *
|
||||
|
||||
# IDL for unitree_hg
|
||||
from .unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_
|
||||
from .unitree_hg.msg.dds_ import LowState_ as HGLowState_
|
||||
from .unitree_hg.msg.dds_ import MotorCmd_ as HGMotorCmd_
|
||||
from .unitree_hg.msg.dds_ import MotorState_ as HGMotorState_
|
||||
from .unitree_hg.msg.dds_ import BmsState_ as HGBmsState_
|
||||
from .unitree_hg.msg.dds_ import IMUState_ as HGIMUState_
|
||||
from .unitree_hg.msg.dds_ import MainBoardState_ as HGMainBoardState_
|
||||
from .unitree_hg.msg.dds_ import PressSensorState_ as HGPressSensorState_
|
||||
from .unitree_hg.msg.dds_ import HandCmd_ as HGHandCmd_
|
||||
from .unitree_hg.msg.dds_ import HandState_ as HGHandState_
|
||||
|
||||
"""
|
||||
" builtin_interfaces_msgs.msg.dds_ dafault
|
||||
"""
|
||||
def builtin_interfaces_msgs_msg_dds__Time_():
|
||||
return Time_(0, 0)
|
||||
|
||||
|
||||
"""
|
||||
" std_msgs.msg.dds_ dafault
|
||||
"""
|
||||
def std_msgs_msg_dds__Header_():
|
||||
return Header_(builtin_interfaces_msgs_msg_dds__Time_(), "")
|
||||
|
||||
def std_msgs_msg_dds__String_():
|
||||
return String_("")
|
||||
|
||||
|
||||
"""
|
||||
" geometry_msgs.msg.dds_ dafault
|
||||
"""
|
||||
def geometry_msgs_msg_dds__Point_():
|
||||
return Point_(0.0, 0.0, 0.0)
|
||||
|
||||
def geometry_msgs_msg_dds__Point32_():
|
||||
return Point32_(0.0, 0.0, 0.0)
|
||||
|
||||
def geometry_msgs_msg_dds__PointStamped_():
|
||||
return PointStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Point_())
|
||||
|
||||
def geometry_msgs_msg_dds__Quaternion_():
|
||||
return Quaternion_(0.0, 0.0, 0.0, 0.0)
|
||||
|
||||
def geometry_msgs_msg_dds__Vector3_():
|
||||
return Vector3_(0.0, 0.0, 0.0)
|
||||
|
||||
def geometry_msgs_msg_dds__Pose_():
|
||||
return Pose_(geometry_msgs_msg_dds__Point_(), geometry_msgs_msg_dds__Quaternion_())
|
||||
|
||||
def geometry_msgs_msg_dds__Pose2D_():
|
||||
return Pose2D_(0.0, 0.0, 0.0)
|
||||
|
||||
def geometry_msgs_msg_dds__PoseStamped_():
|
||||
return PoseStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Pose_())
|
||||
|
||||
def geometry_msgs_msg_dds__PoseWithCovariance_():
|
||||
return PoseWithCovariance_(geometry_msgs_msg_dds__Pose_(), [
|
||||
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 geometry_msgs_msg_dds__PoseWithCovarianceStamped_():
|
||||
return PoseWithCovarianceStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__PoseWithCovariance_())
|
||||
|
||||
def geometry_msgs_msg_dds__QuaternionStamped_():
|
||||
return QuaternionStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Quaternion_())
|
||||
|
||||
def geometry_msgs_msg_dds__Twist_():
|
||||
return Twist_(geometry_msgs_msg_dds__Vector3_(), geometry_msgs_msg_dds__Vector3_())
|
||||
|
||||
def geometry_msgs_msg_dds__TwistStamped_():
|
||||
return TwistStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__Twist_())
|
||||
|
||||
def geometry_msgs_msg_dds__TwistWithCovariance_():
|
||||
return TwistWithCovariance_(geometry_msgs_msg_dds__Twist_(), [
|
||||
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 geometry_msgs_msg_dds__TwistWithCovarianceStamped_():
|
||||
return TwistWithCovarianceStamped_(std_msgs_msg_dds__Header_(), geometry_msgs_msg_dds__TwistWithCovariance_())
|
||||
|
||||
|
||||
"""
|
||||
" nav_msgs.msg.dds_ dafault
|
||||
"""
|
||||
def nav_msgs_msg_dds__MapMetaData_():
|
||||
return MapMetaData_(builtin_interfaces_msgs_msg_dds__Time_(), 0, 0, geometry_msgs_msg_dds__Pose_())
|
||||
|
||||
def nav_msgs_msg_dds__OccupancyGrid_():
|
||||
return OccupancyGrid_(std_msgs_msg_dds__Header_(), nav_msgs_msg_dds__MapMetaData_(), [])
|
||||
|
||||
def nav_msgs_msg_dds__Odometry_():
|
||||
return Odometry_(std_msgs_msg_dds__Header_(), "", geometry_msgs_msg_dds__PoseWithCovariance_(),
|
||||
geometry_msgs_msg_dds__TwistWithCovariance_())
|
||||
|
||||
|
||||
"""
|
||||
" sensor_msgs.msg.dds_ dafault
|
||||
"""
|
||||
def sensor_msgs_msg_dds__PointField_Constants_PointField_():
|
||||
return PointField_("", 0, 0, 0)
|
||||
|
||||
def sensor_msgs_msg_dds__PointField_Constants_PointCloud2_():
|
||||
return PointCloud2_(std_msgs_msg_dds__Header_(), 0, 0, [], False, 0, 0, [], False)
|
||||
|
||||
|
||||
"""
|
||||
" unitree_go.msg.dds_ dafault
|
||||
"""
|
||||
def unitree_go_msg_dds__AudioData_():
|
||||
return AudioData_(0, [])
|
||||
|
||||
def unitree_go_msg_dds__BmsCmd_():
|
||||
return BmsCmd_(0, [0, 0, 0])
|
||||
|
||||
def unitree_go_msg_dds__BmsState_():
|
||||
return BmsState_(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_go_msg_dds__Error_():
|
||||
return Error_(0, 0)
|
||||
|
||||
def unitree_go_msg_dds__Go2FrontVideoData_():
|
||||
return Go2FrontVideoData_(0, [], [], [])
|
||||
|
||||
def unitree_go_msg_dds__HeightMap_():
|
||||
return HeightMap_(0.0, "", 0.0, 0, 0, [0.0, 0.0], [])
|
||||
|
||||
def unitree_go_msg_dds__IMUState_():
|
||||
return IMUState_([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_go_msg_dds__InterfaceConfig_():
|
||||
return InterfaceConfig_(0, 0, [0, 0])
|
||||
|
||||
def unitree_go_msg_dds__LidarState_():
|
||||
return LidarState_(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_go_msg_dds__MotorCmd_():
|
||||
return MotorCmd_(0, 0.0, 0.0, 0.0, 0.0, 0.0, [0, 0, 0])
|
||||
|
||||
def unitree_go_msg_dds__MotorState_():
|
||||
return MotorState_(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, [0, 0])
|
||||
|
||||
def unitree_go_msg_dds__LowCmd_():
|
||||
return LowCmd_([0, 0], 0, 0, [0, 0], [0, 0], 0, [unitree_go_msg_dds__MotorCmd_() for i in range(20)],
|
||||
unitree_go_msg_dds__BmsCmd_(),
|
||||
[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_go_msg_dds__LowState_():
|
||||
return LowState_([0, 0], 0, 0, [0, 0], [0, 0], 0, unitree_go_msg_dds__IMUState_(),
|
||||
[unitree_go_msg_dds__MotorState_() for i in range(20)],
|
||||
unitree_go_msg_dds__BmsState_(), [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_go_msg_dds__Req_():
|
||||
return Req_("", "")
|
||||
|
||||
def unitree_go_msg_dds__Res_():
|
||||
return Res_("", [], "")
|
||||
|
||||
def unitree_go_msg_dds__TimeSpec_():
|
||||
return TimeSpec_(0, 0)
|
||||
|
||||
def unitree_go_msg_dds__PathPoint_():
|
||||
return PathPoint_(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
|
||||
def unitree_go_msg_dds__SportModeState_():
|
||||
return SportModeState_(unitree_go_msg_dds__TimeSpec_(), 0, unitree_go_msg_dds__IMUState_(),
|
||||
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.0, 0.0, 0.0, 0.0],[unitree_go_msg_dds__PathPoint_() for i in range(10)])
|
||||
|
||||
def unitree_go_msg_dds__UwbState_():
|
||||
return UwbState_([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_go_msg_dds__UwbSwitch_():
|
||||
return UwbSwitch_(0)
|
||||
|
||||
def unitree_go_msg_dds__WirelessController_():
|
||||
return WirelessController_(0.0, 0.0, 0.0, 0.0, 0)
|
||||
|
||||
|
||||
"""
|
||||
" unitree_hg.msg.dds_ dafault
|
||||
"""
|
||||
def unitree_hg_msg_dds__BmsCmd_():
|
||||
return HGBmsCmd_(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__BmsState_():
|
||||
return HGBmsState_(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__IMUState_():
|
||||
return HGIMUState_([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__MotorCmd_():
|
||||
return HGMotorCmd_(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0)
|
||||
|
||||
def unitree_hg_msg_dds__MotorState_():
|
||||
return HGMotorState_(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__MainBoardState_():
|
||||
return HGMainBoardState_([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__LowCmd_():
|
||||
return HGLowCmd_(0, 0, [unitree_hg_msg_dds__MotorCmd_() for i in range(35)], [0, 0, 0, 0], 0)
|
||||
|
||||
def unitree_hg_msg_dds__LowState_():
|
||||
return HGLowState_([0, 0], 0, 0, 0, unitree_hg_msg_dds__IMUState_(),
|
||||
[unitree_hg_msg_dds__MotorState_() for i in range(35)],
|
||||
[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__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)
|
||||
|
||||
def unitree_hg_msg_dds__HandCmd_():
|
||||
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__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_api.msg.dds_ dafault
|
||||
"""
|
||||
def unitree_api_msg_dds__RequestIdentity_():
|
||||
return RequestIdentity_(0, 0)
|
||||
|
||||
def unitree_api_msg_dds__RequestLease_():
|
||||
return RequestLease_(0, unitree_hg_msg_dds__IMUState_(), [], )
|
||||
|
||||
def unitree_api_msg_dds__RequestPolicy_():
|
||||
return RequestPolicy_(0, False)
|
||||
|
||||
def unitree_api_msg_dds__RequestHeader_():
|
||||
return RequestHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__RequestLease_(),
|
||||
unitree_api_msg_dds__RequestPolicy_())
|
||||
|
||||
def unitree_api_msg_dds__Request_():
|
||||
return Request_(unitree_api_msg_dds__RequestHeader_(), "", [])
|
||||
|
||||
def unitree_api_msg_dds__ResponseStatus_():
|
||||
return ResponseStatus_(0)
|
||||
|
||||
def unitree_api_msg_dds__ResponseHeader_():
|
||||
return ResponseHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__ResponseStatus_())
|
||||
|
||||
def unitree_api_msg_dds__Response_():
|
||||
return Response_(unitree_api_msg_dds__ResponseHeader_(), "", [], 0, 0, [0, 0])
|
||||
|
||||
9
unitree_sdk2py/idl/geometry_msgs/__init__.py
Normal file
9
unitree_sdk2py/idl/geometry_msgs/__init__.py
Normal file
@ -0,0 +1,9 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs
|
||||
|
||||
"""
|
||||
|
||||
from . import msg
|
||||
__all__ = ["msg", ]
|
||||
9
unitree_sdk2py/idl/geometry_msgs/msg/__init__.py
Normal file
9
unitree_sdk2py/idl/geometry_msgs/msg/__init__.py
Normal file
@ -0,0 +1,9 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg
|
||||
|
||||
"""
|
||||
|
||||
from . import dds_
|
||||
__all__ = ["dds_", ]
|
||||
29
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point32_.py
Normal file
29
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point32_.py
Normal file
@ -0,0 +1,29 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg.dds_
|
||||
IDL file: Point32_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import geometry_msgs
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class Point32_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Point32_"):
|
||||
x: types.float32
|
||||
y: types.float32
|
||||
z: types.float32
|
||||
|
||||
|
||||
31
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PointStamped_.py
Normal file
31
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PointStamped_.py
Normal file
@ -0,0 +1,31 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg.dds_
|
||||
IDL file: PointStamped_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import geometry_msgs
|
||||
|
||||
# if TYPE_CHECKING:
|
||||
# import std_msgs.msg.dds_
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class PointStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PointStamped_"):
|
||||
header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_'
|
||||
point: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Point_'
|
||||
|
||||
|
||||
29
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point_.py
Normal file
29
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Point_.py
Normal file
@ -0,0 +1,29 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg.dds_
|
||||
IDL file: Point_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import geometry_msgs
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class Point_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Point_"):
|
||||
x: types.float64
|
||||
y: types.float64
|
||||
z: types.float64
|
||||
|
||||
|
||||
29
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose2D_.py
Normal file
29
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_Pose2D_.py
Normal file
@ -0,0 +1,29 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg.dds_
|
||||
IDL file: Pose2D_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import geometry_msgs
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class Pose2D_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.Pose2D_"):
|
||||
x: types.float64
|
||||
y: types.float64
|
||||
theta: types.float64
|
||||
|
||||
|
||||
32
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseStamped_.py
Normal file
32
unitree_sdk2py/idl/geometry_msgs/msg/dds_/_PoseStamped_.py
Normal file
@ -0,0 +1,32 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg.dds_
|
||||
IDL file: PoseStamped_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import geometry_msgs
|
||||
|
||||
# if TYPE_CHECKING:
|
||||
# import std_msgs.msg.dds_
|
||||
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class PoseStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PoseStamped_"):
|
||||
header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_'
|
||||
pose: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.Pose_'
|
||||
|
||||
|
||||
@ -0,0 +1,32 @@
|
||||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: geometry_msgs.msg.dds_
|
||||
IDL file: PoseWithCovarianceStamped_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
# root module import for resolving types
|
||||
# import geometry_msgs
|
||||
|
||||
# if TYPE_CHECKING:
|
||||
# import std_msgs.msg.dds_
|
||||
|
||||
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class PoseWithCovarianceStamped_(idl.IdlStruct, typename="geometry_msgs.msg.dds_.PoseWithCovarianceStamped_"):
|
||||
header: 'unitree_sdk2py.idl.std_msgs.msg.dds_.Header_'
|
||||
pose: 'unitree_sdk2py.idl.geometry_msgs.msg.dds_.PoseWithCovariance_'
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user