Skip to content

Commit c52f48b

Browse files
xiaoliangstdxiaoliangstd
committed
update high-level and low-level control examples
1 parent 6cf6a18 commit c52f48b

32 files changed

Lines changed: 2107 additions & 484 deletions

‎example/b2/high_level/b2_sport_client.py‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
import time
2+
import sys
3+
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
4+
from unitree_sdk2py.b2.sport.sport_client import SportClient
5+
import math
6+
from dataclasses import dataclass
7+
8+
@dataclass
9+
class TestOption:
10+
name: str
11+
id: int
12+
13+
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)
25+
]
26+
27+
class UserInterface:
28+
def __init__(self):
29+
self.test_option_ = None
30+
31+
def convert_to_int(self, input_str):
32+
try:
33+
return int(input_str)
34+
except ValueError:
35+
return None
36+
37+
def terminal_handle(self):
38+
input_str = input("Enter id or name: \n")
39+
40+
if input_str == "list":
41+
self.test_option_.name = None
42+
self.test_option_.id = None
43+
for option in option_list:
44+
print(f"{option.name}, id: {option.id}")
45+
return
46+
47+
for option in option_list:
48+
if input_str == option.name or self.convert_to_int(input_str) == option.id:
49+
self.test_option_.name = option.name
50+
self.test_option_.id = option.id
51+
print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
52+
return
53+
54+
print("No matching test option found.")
55+
56+
if __name__ == "__main__":
57+
58+
if len(sys.argv) < 2:
59+
print(f"Usage: python3 {sys.argv[0]} networkInterface")
60+
sys.exit(-1)
61+
62+
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
63+
input("Press Enter to continue...")
64+
65+
ChannelFactoryInitialize(0, sys.argv[1])
66+
67+
test_option = TestOption(name=None, id=None)
68+
user_interface = UserInterface()
69+
user_interface.test_option_ = test_option
70+
71+
sport_client = SportClient()
72+
sport_client.SetTimeout(10.0)
73+
sport_client.Init()
74+
75+
print("Input \"list\" to list all test option ...")
76+
77+
while True:
78+
user_interface.terminal_handle()
79+
80+
print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}")
81+
82+
if test_option.id == 0:
83+
sport_client.Damp()
84+
elif test_option.id == 1:
85+
sport_client.StandUp()
86+
elif test_option.id == 2:
87+
sport_client.StandDown()
88+
elif test_option.id == 3:
89+
sport_client.Move(0.3,0,0)
90+
elif test_option.id == 4:
91+
sport_client.Move(0,0.3,0)
92+
elif test_option.id == 5:
93+
sport_client.Move(0,0,0.5)
94+
elif test_option.id == 6:
95+
sport_client.StopMove()
96+
elif test_option.id == 7:
97+
sport_client.SwitchGait(0)
98+
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()
104+
105+
time.sleep(1)

‎example/b2/low_level/b2_stand_example.py‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
import time
2+
import sys
3+
4+
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
5+
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
6+
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
7+
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
8+
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
9+
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
10+
from unitree_sdk2py.utils.thread import RecurrentThread
11+
import unitree_legged_const as b2
12+
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
13+
from unitree_sdk2py.b2.sport.sport_client import SportClient
14+
15+
from unitree_sdk2py.utils.crc import CRC
16+
17+
class Custom:
18+
def __init__(self):
19+
self.Kp = 1000.0
20+
self.Kd = 10.0
21+
self.time_consume = 0
22+
self.rate_count = 0
23+
self.sin_count = 0
24+
self.motiontime = 0
25+
self.dt = 0.002
26+
27+
self.low_cmd = unitree_go_msg_dds__LowCmd_()
28+
self.low_state = None
29+
30+
self.targetPos_1 = [0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
31+
-0.2, 1.36, -2.65, 0.2, 1.36, -2.65]
32+
33+
self.targetPos_2 = [0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
34+
0.0, 0.67, -1.3, 0.0, 0.67, -1.3]
35+
36+
self.targetPos_3 = [-0.5, 1.36, -2.65, 0.5, 1.36, -2.65,
37+
-0.5, 1.36, -2.65, 0.5, 1.36, -2.65]
38+
39+
self.startPos = [0.0] * 12
40+
self.duration_1 = 500
41+
self.duration_2 = 900
42+
self.duration_3 = 1000
43+
self.duration_4 = 900
44+
self.percent_1 = 0
45+
self.percent_2 = 0
46+
self.percent_3 = 0
47+
self.percent_4 = 0
48+
49+
self.firstRun = True
50+
self.done = False
51+
52+
# thread handling
53+
self.lowCmdWriteThreadPtr = None
54+
55+
self.crc = CRC()
56+
57+
def Init(self):
58+
self.InitLowCmd()
59+
60+
# create publisher #
61+
self.lowcmd_publisher = ChannelPublisher("rt/lowcmd", LowCmd_)
62+
self.lowcmd_publisher.Init()
63+
64+
# create subscriber #
65+
self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
66+
self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
67+
68+
self.sc = SportClient()
69+
self.sc.SetTimeout(5.0)
70+
self.sc.Init()
71+
72+
self.msc = MotionSwitcherClient()
73+
self.msc.SetTimeout(5.0)
74+
self.msc.Init()
75+
76+
status, result = self.msc.CheckMode()
77+
while result['name']:
78+
self.sc.StandDown()
79+
self.msc.ReleaseMode()
80+
status, result = self.msc.CheckMode()
81+
time.sleep(1)
82+
83+
84+
def Start(self):
85+
self.lowCmdWriteThreadPtr = RecurrentThread(
86+
interval=0.002, target=self.LowCmdWrite, name="writebasiccmd"
87+
)
88+
self.lowCmdWriteThreadPtr.Start()
89+
90+
def InitLowCmd(self):
91+
self.low_cmd.head[0] = 0xFE
92+
self.low_cmd.head[1] = 0xEF
93+
self.low_cmd.level_flag = 0xFF
94+
self.low_cmd.gpio = 0
95+
for i in range(20):
96+
self.low_cmd.motor_cmd[i].mode = 0x01
97+
self.low_cmd.motor_cmd[i].q= b2.PosStopF
98+
self.low_cmd.motor_cmd[i].kp = 0
99+
self.low_cmd.motor_cmd[i].dq = b2.VelStopF
100+
self.low_cmd.motor_cmd[i].kd = 0
101+
self.low_cmd.motor_cmd[i].tau = 0
102+
103+
def LowStateMessageHandler(self, msg: LowState_):
104+
self.low_state = msg
105+
106+
def LowCmdWrite(self):
107+
108+
if self.firstRun:
109+
for i in range(12):
110+
self.startPos[i] = self.low_state.motor_state[i].q
111+
self.firstRun = False
112+
113+
self.percent_1 += 1.0 / self.duration_1
114+
self.percent_1 = min(self.percent_1, 1)
115+
if self.percent_1 < 1:
116+
for i in range(12):
117+
self.low_cmd.motor_cmd[i].q = (1 - self.percent_1) * self.startPos[i] + self.percent_1 * self.targetPos_1[i]
118+
self.low_cmd.motor_cmd[i].dq = 0
119+
self.low_cmd.motor_cmd[i].kp = self.Kp
120+
self.low_cmd.motor_cmd[i].kd = self.Kd
121+
self.low_cmd.motor_cmd[i].tau = 0
122+
123+
if (self.percent_1 == 1) and (self.percent_2 <= 1):
124+
self.percent_2 += 1.0 / self.duration_2
125+
self.percent_2 = min(self.percent_2, 1)
126+
for i in range(12):
127+
self.low_cmd.motor_cmd[i].q = (1 - self.percent_2) * self.targetPos_1[i] + self.percent_2 * self.targetPos_2[i]
128+
self.low_cmd.motor_cmd[i].dq = 0
129+
self.low_cmd.motor_cmd[i].kp = self.Kp
130+
self.low_cmd.motor_cmd[i].kd = self.Kd
131+
self.low_cmd.motor_cmd[i].tau = 0
132+
133+
if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 < 1):
134+
self.percent_3 += 1.0 / self.duration_3
135+
self.percent_3 = min(self.percent_3, 1)
136+
for i in range(12):
137+
self.low_cmd.motor_cmd[i].q = self.targetPos_2[i]
138+
self.low_cmd.motor_cmd[i].dq = 0
139+
self.low_cmd.motor_cmd[i].kp = self.Kp
140+
self.low_cmd.motor_cmd[i].kd = self.Kd
141+
self.low_cmd.motor_cmd[i].tau = 0
142+
143+
if (self.percent_1 == 1) and (self.percent_2 == 1) and (self.percent_3 == 1) and (self.percent_4 <= 1):
144+
self.percent_4 += 1.0 / self.duration_4
145+
self.percent_4 = min(self.percent_4, 1)
146+
for i in range(12):
147+
self.low_cmd.motor_cmd[i].q = (1 - self.percent_4) * self.targetPos_2[i] + self.percent_4 * self.targetPos_3[i]
148+
self.low_cmd.motor_cmd[i].dq = 0
149+
self.low_cmd.motor_cmd[i].kp = self.Kp
150+
self.low_cmd.motor_cmd[i].kd = self.Kd
151+
self.low_cmd.motor_cmd[i].tau = 0
152+
153+
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
154+
self.lowcmd_publisher.Write(self.low_cmd)
155+
156+
if __name__ == '__main__':
157+
158+
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
159+
input("Press Enter to continue...")
160+
161+
if len(sys.argv)>1:
162+
ChannelFactoryInitialize(0, sys.argv[1])
163+
else:
164+
ChannelFactoryInitialize(0)
165+
166+
custom = Custom()
167+
custom.Init()
168+
custom.Start()
169+
170+
while True:
171+
if custom.percent_4 == 1.0:
172+
time.sleep(1)
173+
print("Done!")
174+
sys.exit(-1)
175+
time.sleep(1)

example/low_level/unitree_legged_const.py renamed to example/b2/low_level/unitree_legged_const.py

File renamed without changes.

‎example/b2w/high_level/b2w_sport_client.py‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
import time
2+
import sys
3+
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
4+
from unitree_sdk2py.b2.sport.sport_client import SportClient
5+
import math
6+
from dataclasses import dataclass
7+
8+
@dataclass
9+
class TestOption:
10+
name: str
11+
id: int
12+
13+
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+
]
25+
26+
class UserInterface:
27+
def __init__(self):
28+
self.test_option_ = None
29+
30+
def convert_to_int(self, input_str):
31+
try:
32+
return int(input_str)
33+
except ValueError:
34+
return None
35+
36+
def terminal_handle(self):
37+
input_str = input("Enter id or name: \n")
38+
39+
if input_str == "list":
40+
self.test_option_.name = None
41+
self.test_option_.id = None
42+
for option in option_list:
43+
print(f"name: {option.name}, id: {option.id}")
44+
return
45+
46+
for option in option_list:
47+
if input_str == option.name or self.convert_to_int(input_str) == option.id:
48+
self.test_option_.name = option.name
49+
self.test_option_.id = option.id
50+
print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
51+
return
52+
53+
print("No matching test option found.")
54+
55+
if __name__ == "__main__":
56+
if len(sys.argv) < 2:
57+
print(f"Usage: python3 {sys.argv[0]} networkInterface")
58+
sys.exit(-1)
59+
60+
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
61+
input("Press Enter to continue...")
62+
63+
ChannelFactoryInitialize(0, sys.argv[1])
64+
65+
test_option = TestOption(name=None, id=None)
66+
user_interface = UserInterface()
67+
user_interface.test_option_ = test_option
68+
69+
sport_client = SportClient()
70+
sport_client.SetTimeout(10.0)
71+
sport_client.Init()
72+
73+
print("Input \"list\" to list all test option ...")
74+
75+
while True:
76+
user_interface.terminal_handle()
77+
78+
print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}\n")
79+
80+
if test_option.id == 0:
81+
sport_client.Damp()
82+
elif test_option.id == 1:
83+
sport_client.StandUp()
84+
elif test_option.id == 2:
85+
sport_client.StandDown()
86+
elif test_option.id == 3:
87+
sport_client.Move(0.3,0,0)
88+
elif test_option.id == 4:
89+
sport_client.Move(0,0.3,0)
90+
elif test_option.id == 5:
91+
sport_client.Move(0,0,0.5)
92+
elif test_option.id == 6:
93+
sport_client.StopMove()
94+
elif test_option.id == 7:
95+
sport_client.SwitchGait(0)
96+
elif test_option.id == 8:
97+
sport_client.SwitchGait(1)
98+
elif test_option.id == 9:
99+
sport_client.RecoveryStand()
100+
101+
time.sleep(1)

0 commit comments

Comments
 (0)