Skip to content

Commit f559291

Browse files
committed
Update the G1 audio client WAV playback example, and also update the B2 locomotion interfaces
1 parent c5754b8 commit f559291

7 files changed

Lines changed: 329 additions & 153 deletions

File tree

example/b2/high_level/b2_sport_client.py

Lines changed: 26 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -11,17 +11,15 @@ class TestOption:
1111
id: int
1212

1313
option_list = [
14-
TestOption(name="damp", id=0),
15-
TestOption(name="stand_up", id=1),
16-
TestOption(name="stand_down", id=2),
17-
TestOption(name="move forward", id=3),
18-
TestOption(name="move lateral", id=4),
19-
TestOption(name="move rotate", id=5),
20-
TestOption(name="stop_move", id=6),
21-
TestOption(name="switch_gait", id=7),
22-
TestOption(name="switch_gait", id=8),
23-
TestOption(name="recovery", id=9),
24-
TestOption(name="balanced stand", id=10)
14+
TestOption(name="Damp", id=0),
15+
TestOption(name="BalanceStand", id=1),
16+
TestOption(name="StopMove", id=2),
17+
TestOption(name="StandUp", id=3),
18+
TestOption(name="StandDown", id=4),
19+
TestOption(name="RecoveryStand", id=5),
20+
TestOption(name="Move", id=6),
21+
TestOption(name="FreeWalk", id=7),
22+
TestOption(name="ClassicWalk", id=8)
2523
]
2624

2725
class UserInterface:
@@ -59,7 +57,9 @@ def terminal_handle(self):
5957
print(f"Usage: python3 {sys.argv[0]} networkInterface")
6058
sys.exit(-1)
6159

62-
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
60+
print("WARNING: Please ensure there are no obstacles around the robot while running this example.\n"
61+
"NOTE: Some interfaces are not demonstrated in this example for safety reasons. "
62+
"If you need to use them, please contact technical support: https://serviceconsole.unitree.com/index.html#/")
6363
input("Press Enter to continue...")
6464

6565
ChannelFactoryInitialize(0, sys.argv[1])
@@ -80,26 +80,27 @@ def terminal_handle(self):
8080
print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}")
8181

8282
if test_option.id == 0:
83-
sport_client.Damp()
83+
print(f"ret:{sport_client.Damp()}")
8484
elif test_option.id == 1:
85-
sport_client.StandUp()
85+
print(f"ret:{sport_client.BalanceStand()}")
8686
elif test_option.id == 2:
87-
sport_client.StandDown()
87+
print(f"ret:{sport_client.StopMove()}")
8888
elif test_option.id == 3:
89-
sport_client.Move(0.3,0,0)
89+
print(f"ret:{sport_client.StandUp()}")
9090
elif test_option.id == 4:
91-
sport_client.Move(0,0.3,0)
91+
print(f"ret:{sport_client.StandDown()}")
9292
elif test_option.id == 5:
93-
sport_client.Move(0,0,0.5)
93+
print(f"ret:{sport_client.RecoveryStand()}")
9494
elif test_option.id == 6:
95-
sport_client.StopMove()
95+
print(f"ret:{sport_client.Move(0.5,0.0,0.0)}")
9696
elif test_option.id == 7:
97-
sport_client.SwitchGait(0)
97+
print(f"ret:{sport_client.FreeWalk()}")
9898
elif test_option.id == 8:
99-
sport_client.SwitchGait(1)
100-
elif test_option.id == 9:
101-
sport_client.RecoveryStand()
102-
elif test_option.id == 10:
103-
sport_client.BalanceStand()
99+
print(f"ret:{sport_client.ClassicWalk(True)}")
100+
print(f"ret:{sport_client.Move(0.1,0.0,0.0)}")
101+
time.sleep(2)
102+
print(f"ret:{sport_client.ClassicWalk(False)}")
103+
print(f"ret:{sport_client.Move(-0.3,0.0,0.0)}")
104+
time.sleep(2)
104105

105106
time.sleep(1)
Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
import sys
2+
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
3+
from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient
4+
from wav import read_wav, play_pcm_stream
5+
6+
def main():
7+
if len(sys.argv) < 3:
8+
print(f"Usage: python3 {sys.argv[0]} <network_interface> <wav_file_path>")
9+
sys.exit(1)
10+
11+
net_interface = sys.argv[1]
12+
wav_path = sys.argv[2]
13+
14+
ChannelFactoryInitialize(0, net_interface)
15+
audioClient = AudioClient()
16+
audioClient.SetTimeout(10.0)
17+
audioClient.Init()
18+
19+
pcm_list, sample_rate, num_channels, is_ok = read_wav(wav_path)
20+
print(f"[DEBUG] Read success: {is_ok}")
21+
print(f"[DEBUG] Sample rate: {sample_rate} Hz")
22+
print(f"[DEBUG] Channels: {num_channels}")
23+
print(f"[DEBUG] PCM byte length: {len(pcm_list)}")
24+
25+
if not is_ok or sample_rate != 16000 or num_channels != 1:
26+
print("[ERROR] Failed to read WAV file or unsupported format (must be 16kHz mono)")
27+
return
28+
29+
play_pcm_stream(audioClient, pcm_list, "example")
30+
31+
audioClient.PlayStop("example")
32+
33+
if __name__ == "__main__":
34+
main()
35+

example/g1/audio/test.wav

129 KB
Binary file not shown.

example/g1/audio/wav.py

Lines changed: 166 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
import struct
2+
import time
3+
4+
def read_wav(filename):
5+
try:
6+
with open(filename, 'rb') as f:
7+
def read(fmt):
8+
return struct.unpack(fmt, f.read(struct.calcsize(fmt)))
9+
10+
# === Chunk Header ===
11+
chunk_id, = read('<I')
12+
if chunk_id != 0x46464952: # "RIFF"
13+
print(f"[ERROR] chunk_id != 'RIFF': {hex(chunk_id)}")
14+
return [], -1, -1, False
15+
16+
_chunk_size, = read('<I')
17+
format_tag, = read('<I')
18+
if format_tag != 0x45564157: # "WAVE"
19+
print(f"[ERROR] format != 'WAVE': {hex(format_tag)}")
20+
return [], -1, -1, False
21+
22+
# === Subchunk1: fmt ===
23+
subchunk1_id, = read('<I')
24+
subchunk1_size, = read('<I')
25+
26+
if subchunk1_id == 0x4B4E554A: # JUNK
27+
f.seek(subchunk1_size, 1)
28+
subchunk1_id, = read('<I')
29+
subchunk1_size, = read('<I')
30+
31+
if subchunk1_id != 0x20746D66: # "fmt "
32+
print(f"[ERROR] subchunk1_id != 'fmt ': {hex(subchunk1_id)}")
33+
return [], -1, -1, False
34+
35+
if subchunk1_size not in [16, 18]:
36+
print(f"[ERROR] subchunk1_size != 16 or 18: {subchunk1_size}")
37+
return [], -1, -1, False
38+
39+
audio_format, = read('<H')
40+
if audio_format != 1:
41+
print(f"[ERROR] audio_format != PCM (1): {audio_format}")
42+
return [], -1, -1, False
43+
44+
num_channels, = read('<H')
45+
sample_rate, = read('<I')
46+
byte_rate, = read('<I')
47+
block_align, = read('<H')
48+
bits_per_sample, = read('<H')
49+
50+
expected_byte_rate = sample_rate * num_channels * bits_per_sample // 8
51+
if byte_rate != expected_byte_rate:
52+
print(f"[ERROR] byte_rate mismatch: got {byte_rate}, expected {expected_byte_rate}")
53+
return [], -1, -1, False
54+
55+
expected_align = num_channels * bits_per_sample // 8
56+
if block_align != expected_align:
57+
print(f"[ERROR] block_align mismatch: got {block_align}, expected {expected_align}")
58+
return [], -1, -1, False
59+
60+
if bits_per_sample != 16:
61+
print(f"[ERROR] Only 16-bit samples supported, got {bits_per_sample}")
62+
return [], -1, -1, False
63+
64+
if subchunk1_size == 18:
65+
extra_size, = read('<H')
66+
if extra_size != 0:
67+
print(f"[ERROR] extra_size != 0: {extra_size}")
68+
return [], -1, -1, False
69+
70+
# === Subchunk2: data ===
71+
while True:
72+
subchunk2_id, subchunk2_size = read('<II')
73+
if subchunk2_id == 0x61746164: # "data"
74+
break
75+
f.seek(subchunk2_size, 1)
76+
77+
raw_pcm = f.read(subchunk2_size)
78+
if len(raw_pcm) != subchunk2_size:
79+
print("[ERROR] Failed to read full PCM data")
80+
return [], -1, -1, False
81+
82+
return list(raw_pcm), sample_rate, num_channels, True
83+
84+
except Exception as e:
85+
print(f"[ERROR] read_wave() failed: {e}")
86+
return [], -1, -1, False
87+
88+
89+
def write_wave(filename, sample_rate, samples, num_channels=1):
90+
try:
91+
import array
92+
if isinstance(samples[0], int):
93+
samples = array.array('h', samples)
94+
95+
subchunk2_size = len(samples) * 2
96+
chunk_size = 36 + subchunk2_size
97+
98+
with open(filename, 'wb') as f:
99+
# RIFF chunk
100+
f.write(struct.pack('<I', 0x46464952)) # "RIFF"
101+
f.write(struct.pack('<I', chunk_size))
102+
f.write(struct.pack('<I', 0x45564157)) # "WAVE"
103+
104+
# fmt subchunk
105+
f.write(struct.pack('<I', 0x20746D66)) # "fmt "
106+
f.write(struct.pack('<I', 16)) # PCM
107+
f.write(struct.pack('<H', 1)) # PCM format
108+
f.write(struct.pack('<H', num_channels))
109+
f.write(struct.pack('<I', sample_rate))
110+
f.write(struct.pack('<I', sample_rate * num_channels * 2)) # byte_rate
111+
f.write(struct.pack('<H', num_channels * 2)) # block_align
112+
f.write(struct.pack('<H', 16)) # bits per sample
113+
114+
# data subchunk
115+
f.write(struct.pack('<I', 0x61746164)) # "data"
116+
f.write(struct.pack('<I', subchunk2_size))
117+
f.write(samples.tobytes())
118+
119+
return True
120+
except Exception as e:
121+
print(f"[ERROR] write_wave() failed: {e}")
122+
return False
123+
124+
125+
def play_pcm_stream(client, pcm_list, stream_name="example", chunk_size=96000, sleep_time=1.0, verbose=False):
126+
"""
127+
Play PCM audio stream (16-bit little-endian format), sending data in chunks.
128+
129+
Parameters:
130+
client: An object with a PlayStream method
131+
pcm_list: list[int], PCM audio data in int16 format
132+
stream_name: Stream name, default is "example"
133+
chunk_size: Number of bytes to send per chunk, default is 96000 (3 seconds at 16kHz)
134+
sleep_time: Delay between chunks in seconds
135+
"""
136+
pcm_data = bytes(pcm_list)
137+
stream_id = str(int(time.time() * 1000)) # Unique stream ID based on current timestamp
138+
offset = 0
139+
chunk_index = 0
140+
total_size = len(pcm_data)
141+
142+
while offset < total_size:
143+
remaining = total_size - offset
144+
current_chunk_size = min(chunk_size, remaining)
145+
chunk = pcm_data[offset:offset + current_chunk_size]
146+
147+
if verbose:
148+
# Print info about the current chunk
149+
print(f"[CHUNK {chunk_index}] offset = {offset}, size = {current_chunk_size} bytes")
150+
print(" First 10 samples (int16): ", end="")
151+
for i in range(0, min(20, len(chunk) - 1), 2):
152+
sample = struct.unpack_from('<h', chunk, i)[0]
153+
print(sample, end=" ")
154+
print()
155+
156+
# Send the chunk
157+
ret_code, _ = client.PlayStream(stream_name, stream_id, chunk)
158+
if ret_code != 0:
159+
print(f"[ERROR] Failed to send chunk {chunk_index}, return code: {ret_code}")
160+
break
161+
else:
162+
print(f"[INFO] Chunk {chunk_index} sent successfully")
163+
164+
offset += current_chunk_size
165+
chunk_index += 1
166+
time.sleep(sleep_time)

unitree_sdk2py/b2/sport/sport_api.py

Lines changed: 21 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -13,40 +13,27 @@
1313
"""
1414
" api id
1515
"""
16-
SPORT_API_ID_DAMP = 1001;
17-
SPORT_API_ID_BALANCESTAND = 1002;
18-
SPORT_API_ID_STOPMOVE = 1003;
19-
SPORT_API_ID_STANDUP = 1004;
20-
SPORT_API_ID_STANDDOWN = 1005;
21-
SPORT_API_ID_RECOVERYSTAND = 1006;
22-
SPORT_API_ID_EULER = 1007;
23-
SPORT_API_ID_MOVE = 1008;
24-
SPORT_API_ID_SIT = 1009;
25-
SPORT_API_ID_RISESIT = 1010;
26-
SPORT_API_ID_SWITCHGAIT = 1011;
27-
SPORT_API_ID_TRIGGER = 1012;
28-
SPORT_API_ID_BODYHEIGHT = 1013;
29-
SPORT_API_ID_FOOTRAISEHEIGHT = 1014;
30-
SPORT_API_ID_SPEEDLEVEL = 1015;
31-
SPORT_API_ID_HELLO = 1016;
32-
SPORT_API_ID_STRETCH = 1017;
33-
SPORT_API_ID_TRAJECTORYFOLLOW = 1018;
34-
SPORT_API_ID_CONTINUOUSGAIT = 1019;
35-
SPORT_API_ID_CONTENT = 1020;
36-
SPORT_API_ID_WALLOW = 1021;
37-
SPORT_API_ID_DANCE1 = 1022;
38-
SPORT_API_ID_DANCE2 = 1023;
39-
SPORT_API_ID_GETBODYHEIGHT = 1024;
40-
SPORT_API_ID_GETFOOTRAISEHEIGHT = 1025;
41-
SPORT_API_ID_GETSPEEDLEVEL = 1026;
42-
SPORT_API_ID_SWITCHJOYSTICK = 1027;
43-
SPORT_API_ID_POSE = 1028;
44-
SPORT_API_ID_FRONTJUMP = 1031;
45-
SPORT_API_ID_ECONOMICGAIT = 1035;
46-
SPORT_API_ID_MOVETOPOS = 1036;
47-
SPORT_API_ID_SWITCHEULERMODE = 1037;
48-
SPORT_API_ID_SWITCHMOVEMODE = 1038;
49-
16+
ROBOT_SPORT_API_ID_DAMP = 1001
17+
ROBOT_SPORT_API_ID_BALANCESTAND = 1002
18+
ROBOT_SPORT_API_ID_STOPMOVE = 1003
19+
ROBOT_SPORT_API_ID_STANDUP = 1004
20+
ROBOT_SPORT_API_ID_STANDDOWN = 1005
21+
ROBOT_SPORT_API_ID_RECOVERYSTAND = 1006
22+
ROBOT_SPORT_API_ID_MOVE = 1008
23+
ROBOT_SPORT_API_ID_SWITCHGAIT = 1011
24+
ROBOT_SPORT_API_ID_BODYHEIGHT = 1013
25+
ROBOT_SPORT_API_ID_SPEEDLEVEL = 1015
26+
ROBOT_SPORT_API_ID_TRAJECTORYFOLLOW = 1018
27+
ROBOT_SPORT_API_ID_CONTINUOUSGAIT = 1019
28+
ROBOT_SPORT_API_ID_MOVETOPOS = 1036
29+
ROBOT_SPORT_API_ID_SWITCHMOVEMODE = 1038
30+
ROBOT_SPORT_API_ID_VISIONWALK = 1101
31+
ROBOT_SPORT_API_ID_HANDSTAND = 1039
32+
ROBOT_SPORT_API_ID_AUTORECOVERY_SET = 1040
33+
ROBOT_SPORT_API_ID_FREEWALK = 1045
34+
ROBOT_SPORT_API_ID_CLASSICWALK = 1049
35+
ROBOT_SPORT_API_ID_FASTWALK = 1050
36+
ROBOT_SPORT_API_ID_FREEEULER = 1051
5037

5138
"""
5239
" error code

0 commit comments

Comments
 (0)