Merge remote-tracking branch 'upstream/master'
update to original remote repo
This commit is contained in:
jgonzalez
commit
9b05080718
|
|
@ -34,3 +34,6 @@ __pycache__
|
|||
|
||||
# JetBrains IDE
|
||||
.idea/
|
||||
|
||||
# python
|
||||
dist/
|
||||
|
|
@ -8,6 +8,12 @@ Python interface for unitree sdk2
|
|||
- numpy
|
||||
- opencv-python
|
||||
## Install unitree_sdk2_python
|
||||
|
||||
```bash
|
||||
pip install unitree_sdk2py
|
||||
```
|
||||
|
||||
### Installing from source
|
||||
Execute the following commands in the terminal:
|
||||
```bash
|
||||
cd ~
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
import time
|
||||
import sys
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
|
||||
from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
@dataclass
|
||||
class TestOption:
|
||||
name: str
|
||||
id: int
|
||||
|
||||
option_list = [
|
||||
TestOption(name="damp", id=0),
|
||||
TestOption(name="stand_up", id=1),
|
||||
TestOption(name="sit", id=2),
|
||||
TestOption(name="move forward", id=3),
|
||||
TestOption(name="move lateral", id=4),
|
||||
TestOption(name="move rotate", id=5),
|
||||
TestOption(name="low stand", id=6),
|
||||
TestOption(name="high stand", id=7),
|
||||
TestOption(name="zero torque", id=8),
|
||||
TestOption(name="wave hand1", id=9), # wave hand without turning around
|
||||
TestOption(name="wave hand2", id=10), # wave hand and trun around
|
||||
TestOption(name="shake hand", id=11),
|
||||
]
|
||||
|
||||
class UserInterface:
|
||||
def __init__(self):
|
||||
self.test_option_ = None
|
||||
|
||||
def convert_to_int(self, input_str):
|
||||
try:
|
||||
return int(input_str)
|
||||
except ValueError:
|
||||
return None
|
||||
|
||||
def terminal_handle(self):
|
||||
input_str = input("Enter id or name: \n")
|
||||
|
||||
if input_str == "list":
|
||||
self.test_option_.name = None
|
||||
self.test_option_.id = None
|
||||
for option in option_list:
|
||||
print(f"{option.name}, id: {option.id}")
|
||||
return
|
||||
|
||||
for option in option_list:
|
||||
if input_str == option.name or self.convert_to_int(input_str) == option.id:
|
||||
self.test_option_.name = option.name
|
||||
self.test_option_.id = option.id
|
||||
print(f"Test: {self.test_option_.name}, test_id: {self.test_option_.id}")
|
||||
return
|
||||
|
||||
print("No matching test option found.")
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) < 2:
|
||||
print(f"Usage: python3 {sys.argv[0]} networkInterface")
|
||||
sys.exit(-1)
|
||||
|
||||
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
|
||||
input("Press Enter to continue...")
|
||||
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
|
||||
test_option = TestOption(name=None, id=None)
|
||||
user_interface = UserInterface()
|
||||
user_interface.test_option_ = test_option
|
||||
|
||||
sport_client = LocoClient()
|
||||
sport_client.SetTimeout(10.0)
|
||||
sport_client.Init()
|
||||
|
||||
print("Input \"list\" to list all test option ...")
|
||||
while True:
|
||||
user_interface.terminal_handle()
|
||||
|
||||
print(f"Updated Test Option: Name = {test_option.name}, ID = {test_option.id}")
|
||||
|
||||
if test_option.id == 0:
|
||||
sport_client.Damp()
|
||||
elif test_option.id == 1:
|
||||
sport_client.StandUp()
|
||||
elif test_option.id == 2:
|
||||
sport_client.Sit()
|
||||
elif test_option.id == 3:
|
||||
sport_client.Move(0.3,0,0)
|
||||
elif test_option.id == 4:
|
||||
sport_client.Move(0,0.3,0)
|
||||
elif test_option.id == 5:
|
||||
sport_client.Move(0,0,0.3)
|
||||
elif test_option.id == 6:
|
||||
sport_client.LowStand()
|
||||
elif test_option.id == 7:
|
||||
sport_client.HighStand()
|
||||
elif test_option.id == 8:
|
||||
sport_client.ZeroTorque()
|
||||
elif test_option.id == 9:
|
||||
sport_client.WaveHand()
|
||||
elif test_option.id == 10:
|
||||
sport_client.WaveHand(True)
|
||||
elif test_option.id == 11:
|
||||
sport_client.ShakeHand()
|
||||
time.sleep(3)
|
||||
sport_client.ShakeHand()
|
||||
|
||||
time.sleep(1)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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")
|
||||
|
||||
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
HIGHLEVEL = 0xEE
|
||||
LOWLEVEL = 0xFF
|
||||
TRIGERLEVEL = 0xF0
|
||||
PosStopF = 2.146e9
|
||||
VelStopF = 16000.0
|
||||
|
|
@ -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)
|
||||
|
|
@ -1,133 +0,0 @@
|
|||
import time
|
||||
import numpy as np
|
||||
from enum import IntEnum
|
||||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
|
||||
|
||||
# from user_data import *
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import MotorCmd_
|
||||
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
|
||||
import unitree_sdk2py.idl.unitree_hg.msg.dds_ as dds
|
||||
print(dds.LowCmd_)
|
||||
print(dds.MotorCmd_)
|
||||
|
||||
kTopicArmSDK = "rt/arm_sdk"
|
||||
kPi = 3.141592654
|
||||
kPi_2 = 1.57079632
|
||||
|
||||
|
||||
class JointIndex(IntEnum):
|
||||
|
||||
# // Left arm
|
||||
kLeftShoulderPitch = 13
|
||||
kLeftShoulderRoll = 14
|
||||
kLeftShoulderYaw = 15
|
||||
kLeftElbow = 16
|
||||
kLeftWistYaw = 17
|
||||
kLeftWistPitch = 18
|
||||
kLeftWistRoll = 19
|
||||
# // Right arm
|
||||
kRightShoulderPitch = 20
|
||||
kRightShoulderRoll = 21
|
||||
kRightShoulderYaw = 22
|
||||
kRightElbow = 23
|
||||
kRightWistYaw = 24
|
||||
kRightWistPitch = 25
|
||||
kRightWistRoll = 26
|
||||
|
||||
kWaistYaw = 12
|
||||
|
||||
|
||||
kNotUsedJoint = 27
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ChannelFactoryInitialize()
|
||||
|
||||
# Create a publisher to publish the data defined in UserData class
|
||||
arm_sdk_publisher = ChannelPublisher('rt/arm_sdk', LowCmd_)
|
||||
# pub = ChannelPublisher("rt/lowcmd", LowCmd_)
|
||||
arm_sdk_publisher.Init()
|
||||
|
||||
msg = unitree_hg_msg_dds__LowCmd_()
|
||||
|
||||
weight = 0
|
||||
weight_rate = 0.2
|
||||
|
||||
kp = 60
|
||||
kd = 1.5
|
||||
dq = 0
|
||||
tau_ff = 0
|
||||
|
||||
control_dt = 0.02
|
||||
max_joint_velocity = 0.5
|
||||
|
||||
delta_weight = weight_rate * control_dt
|
||||
max_joint_delta = max_joint_velocity * control_dt
|
||||
|
||||
init_pos = np.zeros(15)
|
||||
target_pos = np.array(
|
||||
[
|
||||
0.0,
|
||||
kPi_2,
|
||||
0.0,
|
||||
kPi_2,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-kPi_2,
|
||||
0.0,
|
||||
kPi_2,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
]
|
||||
)
|
||||
print("Initailizing arms ...")
|
||||
|
||||
init_time = 5
|
||||
init_time_steps = int(init_time / control_dt)
|
||||
|
||||
for i in range(init_time_steps):
|
||||
weight += delta_weight
|
||||
weight = max(min(weight, 1.0), 0)
|
||||
msg.motor_cmd[kNotUsedJoint].q = weight * weight
|
||||
|
||||
for idx, joint in enumerate(JointIndex):
|
||||
msg.motor_cmd[joint].q = init_pos[idx]
|
||||
msg.motor_cmd[joint].dq = dq
|
||||
msg.motor_cmd[joint].kp = kp
|
||||
msg.motor_cmd[joint].kd = kd
|
||||
msg.motor_cmd[joint].tau = tau_ff
|
||||
|
||||
arm_sdk_publisher.Write(msg)
|
||||
|
||||
time.sleep(control_dt)
|
||||
|
||||
print("Done!")
|
||||
|
||||
period = 5
|
||||
num_time_steps = int(period / control_dt)
|
||||
|
||||
current_jpos_des = np.zeros_like(init_pos)
|
||||
|
||||
for i in range(num_time_steps):
|
||||
for j in range(len(current_jpos_des)):
|
||||
delta = target_pos[j] - current_jpos_des[j]
|
||||
clamped_delta = np.clip(delta, -max_joint_delta, max_joint_delta)
|
||||
current_jpos_des[j] += clamped_delta
|
||||
|
||||
for idx, joint in enumerate(JointIndex):
|
||||
msg.motor_cmd[joint].q = current_jpos_des[idx]
|
||||
msg.motor_cmd[joint].dq = dq
|
||||
msg.motor_cmd[joint].kp = kp
|
||||
msg.motor_cmd[joint].kd = kd
|
||||
msg.motor_cmd[joint].tau = tau_ff
|
||||
|
||||
arm_sdk_publisher.Write(msg)
|
||||
|
||||
time.sleep(control_dt)
|
||||
|
||||
exit()
|
||||
|
|
@ -1,50 +0,0 @@
|
|||
import time
|
||||
import numpy as np
|
||||
from enum import IntEnum
|
||||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
|
||||
import sys
|
||||
|
||||
# from user_data import *
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
|
||||
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
|
||||
import unitree_sdk2py.idl.unitree_hg.msg.dds_ as dds
|
||||
from unitree_sdk2py.utils.crc import CRC
|
||||
|
||||
|
||||
kTopicArmSDK = "rt/lowcmd"
|
||||
kPi = 3.141592654
|
||||
kPi_2 = 1.57079632
|
||||
kNumMotors = 35
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
# 初始化发布节点
|
||||
low_cmd_publisher = ChannelPublisher(kTopicArmSDK, LowCmd_)
|
||||
low_cmd_publisher.Init()
|
||||
# 初始化LowCmd_
|
||||
msg = unitree_hg_msg_dds__LowCmd_()
|
||||
# CRC校验实例
|
||||
crc = CRC()
|
||||
# 控制时间参数
|
||||
control_dt = 0.02
|
||||
init_time = 20
|
||||
init_time_steps = int(init_time / control_dt)
|
||||
|
||||
# 控制参数设置
|
||||
msg.mode_pr = 0
|
||||
msg.mode_machine = 4
|
||||
for i in range(init_time_steps):
|
||||
for idx in range(kNumMotors):
|
||||
msg.motor_cmd[idx].mode = 0x01
|
||||
msg.crc = crc.Crc(msg)
|
||||
# print(msg.crc)
|
||||
low_cmd_publisher.Write(msg)
|
||||
time.sleep(control_dt)
|
||||
|
||||
print("Done!")
|
||||
exit()
|
||||
|
|
@ -1,24 +0,0 @@
|
|||
import time
|
||||
import sys
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
|
||||
|
||||
|
||||
def HighStateHandler(msg: SportModeState_):
|
||||
print("Position: ", msg.position)
|
||||
print("Velocity: ", msg.velocity)
|
||||
print("Yaw velocity: ", msg.yaw_speed)
|
||||
print("Foot position in body frame: ", msg.foot_position_body)
|
||||
print("Foot velocity in body frame: ", msg.foot_speed_body)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# sys.argv[1]: name of the network interface
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)
|
||||
sub.Init(HighStateHandler, 10)
|
||||
|
||||
while True:
|
||||
time.sleep(10.0)
|
||||
|
|
@ -1,144 +0,0 @@
|
|||
import time
|
||||
import sys
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
|
||||
from unitree_sdk2py.go2.sport.sport_client import (
|
||||
SportClient,
|
||||
PathPoint,
|
||||
SPORT_PATH_POINT_SIZE,
|
||||
)
|
||||
import math
|
||||
|
||||
|
||||
class SportModeTest:
|
||||
def __init__(self) -> None:
|
||||
# Time count
|
||||
self.t = 0
|
||||
self.dt = 0.01
|
||||
|
||||
# Initial poition and yaw
|
||||
self.px0 = 0
|
||||
self.py0 = 0
|
||||
self.yaw0 = 0
|
||||
|
||||
self.client = SportClient() # Create a sport client
|
||||
self.client.SetTimeout(10.0)
|
||||
self.client.Init()
|
||||
|
||||
def GetInitState(self, robot_state: SportModeState_):
|
||||
self.px0 = robot_state.position[0]
|
||||
self.py0 = robot_state.position[1]
|
||||
self.yaw0 = robot_state.imu_state.rpy[2]
|
||||
|
||||
def StandUpDown(self):
|
||||
self.client.StandDown()
|
||||
print("Stand down !!!")
|
||||
time.sleep(1)
|
||||
|
||||
self.client.StandUp()
|
||||
print("Stand up !!!")
|
||||
time.sleep(1)
|
||||
|
||||
self.client.StandDown()
|
||||
print("Stand down !!!")
|
||||
time.sleep(1)
|
||||
|
||||
self.client.Damp()
|
||||
|
||||
def VelocityMove(self):
|
||||
elapsed_time = 1
|
||||
for i in range(int(elapsed_time / self.dt)):
|
||||
self.client.Move(0.3, 0, 0.3) # vx, vy vyaw
|
||||
time.sleep(self.dt)
|
||||
self.client.StopMove()
|
||||
|
||||
def BalanceAttitude(self):
|
||||
self.client.Euler(0.1, 0.2, 0.3) # roll, pitch, yaw
|
||||
self.client.BalanceStand()
|
||||
|
||||
def TrajectoryFollow(self):
|
||||
time_seg = 0.2
|
||||
time_temp = self.t - time_seg
|
||||
path = []
|
||||
for i in range(SPORT_PATH_POINT_SIZE):
|
||||
time_temp += time_seg
|
||||
|
||||
px_local = 0.5 * math.sin(0.5 * time_temp)
|
||||
py_local = 0
|
||||
yaw_local = 0
|
||||
vx_local = 0.25 * math.cos(0.5 * time_temp)
|
||||
vy_local = 0
|
||||
vyaw_local = 0
|
||||
|
||||
path_point_tmp = PathPoint(0, 0, 0, 0, 0, 0, 0)
|
||||
|
||||
path_point_tmp.timeFromStart = i * time_seg
|
||||
path_point_tmp.x = (
|
||||
px_local * math.cos(self.yaw0)
|
||||
- py_local * math.sin(self.yaw0)
|
||||
+ self.px0
|
||||
)
|
||||
path_point_tmp.y = (
|
||||
px_local * math.sin(self.yaw0)
|
||||
+ py_local * math.cos(self.yaw0)
|
||||
+ self.py0
|
||||
)
|
||||
path_point_tmp.yaw = yaw_local + self.yaw0
|
||||
path_point_tmp.vx = vx_local * math.cos(self.yaw0) - vy_local * math.sin(
|
||||
self.yaw0
|
||||
)
|
||||
path_point_tmp.vy = vx_local * math.sin(self.yaw0) + vy_local * math.cos(
|
||||
self.yaw0
|
||||
)
|
||||
path_point_tmp.vyaw = vyaw_local
|
||||
|
||||
path.append(path_point_tmp)
|
||||
|
||||
self.client.TrajectoryFollow(path)
|
||||
|
||||
def SpecialMotions(self):
|
||||
self.client.RecoveryStand()
|
||||
print("RecoveryStand !!!")
|
||||
time.sleep(1)
|
||||
|
||||
self.client.Stretch()
|
||||
print("Sit !!!")
|
||||
time.sleep(1)
|
||||
|
||||
self.client.RecoveryStand()
|
||||
print("RecoveryStand !!!")
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
# Robot state
|
||||
robot_state = unitree_go_msg_dds__SportModeState_()
|
||||
def HighStateHandler(msg: SportModeState_):
|
||||
global robot_state
|
||||
robot_state = msg
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)
|
||||
sub.Init(HighStateHandler, 10)
|
||||
time.sleep(1)
|
||||
|
||||
test = SportModeTest()
|
||||
test.GetInitState(robot_state)
|
||||
|
||||
print("Start test !!!")
|
||||
while True:
|
||||
test.t += test.dt
|
||||
|
||||
test.StandUpDown()
|
||||
# test.VelocityMove()
|
||||
# test.BalanceAttitude()
|
||||
# test.TrajectoryFollow()
|
||||
# test.SpecialMotions()
|
||||
|
||||
time.sleep(test.dt)
|
||||
|
|
@ -1,60 +0,0 @@
|
|||
import time
|
||||
import sys
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
|
||||
from unitree_sdk2py.utils.crc import CRC
|
||||
from unitree_sdk2py.utils.thread import Thread
|
||||
import unitree_legged_const as go2
|
||||
|
||||
crc = CRC()
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
# Create a publisher to publish the data defined in UserData class
|
||||
pub = ChannelPublisher("rt/lowcmd", LowCmd_)
|
||||
pub.Init()
|
||||
|
||||
cmd = unitree_go_msg_dds__LowCmd_()
|
||||
cmd.head[0]=0xFE
|
||||
cmd.head[1]=0xEF
|
||||
cmd.level_flag = 0xFF
|
||||
cmd.gpio = 0
|
||||
for i in range(20):
|
||||
cmd.motor_cmd[i].mode = 0x01 # (PMSM) mode
|
||||
cmd.motor_cmd[i].q= go2.PosStopF
|
||||
cmd.motor_cmd[i].kp = 0
|
||||
cmd.motor_cmd[i].dq = go2.VelStopF
|
||||
cmd.motor_cmd[i].kd = 0
|
||||
cmd.motor_cmd[i].tau = 0
|
||||
|
||||
while True:
|
||||
# Toque controle, set RL_2 toque
|
||||
cmd.motor_cmd[go2.LegID["RL_2"]].q = 0.0 # Set to stop position(rad)
|
||||
cmd.motor_cmd[go2.LegID["RL_2"]].kp = 0.0
|
||||
cmd.motor_cmd[go2.LegID["RL_2"]].dq = 0.0 # Set to stop angular velocity(rad/s)
|
||||
cmd.motor_cmd[go2.LegID["RL_2"]].kd = 0.0
|
||||
cmd.motor_cmd[go2.LegID["RL_2"]].tau = 1.0 # target toque is set to 1N.m
|
||||
|
||||
# Poinstion(rad) control, set RL_0 rad
|
||||
cmd.motor_cmd[go2.LegID["RL_0"]].q = 0.0 # Taregt angular(rad)
|
||||
cmd.motor_cmd[go2.LegID["RL_0"]].kp = 10.0 # Poinstion(rad) control kp gain
|
||||
cmd.motor_cmd[go2.LegID["RL_0"]].dq = 0.0 # Taregt angular velocity(rad/ss)
|
||||
cmd.motor_cmd[go2.LegID["RL_0"]].kd = 1.0 # Poinstion(rad) control kd gain
|
||||
cmd.motor_cmd[go2.LegID["RL_0"]].tau = 0.0 # Feedforward toque 1N.m
|
||||
|
||||
cmd.crc = crc.Crc(cmd)
|
||||
|
||||
#Publish message
|
||||
if pub.Write(cmd):
|
||||
print("Publish success. msg:", cmd.crc)
|
||||
else:
|
||||
print("Waitting for subscriber.")
|
||||
|
||||
time.sleep(0.002)
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
import time
|
||||
import sys
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
|
||||
|
||||
import unitree_legged_const as go2
|
||||
|
||||
|
||||
def LowStateHandler(msg: LowState_):
|
||||
|
||||
# print front right hip motor states
|
||||
print("FR_0 motor state: ", msg.motor_state[go2.LegID["FR_0"]])
|
||||
print("IMU state: ", msg.imu_state)
|
||||
print("Battery state: voltage: ", msg.power_v, "current: ", msg.power_a)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
sub = ChannelSubscriber("rt/lowstate", LowState_)
|
||||
sub.Init(LowStateHandler, 10)
|
||||
|
||||
while True:
|
||||
time.sleep(10.0)
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
import time
|
||||
import sys
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
|
||||
from unitree_sdk2py.utils.crc import CRC
|
||||
|
||||
crc = CRC()
|
||||
|
||||
PosStopF = 2.146e9
|
||||
VelStopF = 16000.0
|
||||
|
||||
if __name__ == "__main__":
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
cmd = unitree_go_msg_dds__LowCmd_()
|
||||
cmd.head[0] = 0xFE
|
||||
cmd.head[1] = 0xEF
|
||||
cmd.level_flag = 0xFF
|
||||
cmd.gpio = 0
|
||||
for i in range(20):
|
||||
cmd.motor_cmd[i].mode = 0x01
|
||||
cmd.motor_cmd[i].q = PosStopF
|
||||
cmd.motor_cmd[i].kp = 0
|
||||
cmd.motor_cmd[i].dq = VelStopF
|
||||
cmd.motor_cmd[i].kd = 0
|
||||
cmd.motor_cmd[i].tau = 0
|
||||
|
||||
cmd.motor_cmd[0].q = 0.0
|
||||
cmd.motor_cmd[0].kp = 0.0
|
||||
cmd.motor_cmd[0].dq = 0.0
|
||||
cmd.motor_cmd[0].kd = 0.0
|
||||
cmd.motor_cmd[0].tau = 1.0
|
||||
|
||||
cmd.motor_cmd[1].q = 0.0
|
||||
cmd.motor_cmd[1].kp = 10.0
|
||||
cmd.motor_cmd[1].dq = 0.0
|
||||
cmd.motor_cmd[1].kd = 1.0
|
||||
cmd.motor_cmd[1].tau = 0.0
|
||||
|
||||
now = time.perf_counter()
|
||||
|
||||
|
||||
cmd.crc = crc.Crc(cmd)
|
||||
|
||||
print("CRC:", cmd.crc, "Time cost:", (time.perf_counter() - now)*1000)
|
||||
|
|
@ -1,55 +1,131 @@
|
|||
import time
|
||||
import sys
|
||||
import struct
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__WirelessController_
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
|
||||
# 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]
|
||||
|
||||
|
||||
key_state = [
|
||||
["R1", 0],
|
||||
["L1", 0],
|
||||
["start", 0],
|
||||
["select", 0],
|
||||
["R2", 0],
|
||||
["L2", 0],
|
||||
["F1", 0],
|
||||
["F2", 0],
|
||||
["A", 0],
|
||||
["B", 0],
|
||||
["X", 0],
|
||||
["Y", 0],
|
||||
["up", 0],
|
||||
["right", 0],
|
||||
["down", 0],
|
||||
["left", 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)
|
||||
|
||||
|
||||
def WirelessControllerHandler(msg: WirelessController_):
|
||||
global key_state
|
||||
print("lx: ", msg.lx)
|
||||
print("lx: ", msg.ly)
|
||||
print("lx: ", msg.rx)
|
||||
print("lx: ", msg.ry)
|
||||
print("keys: ", msg.keys)
|
||||
if __name__ == '__main__':
|
||||
|
||||
#Update key state
|
||||
for i in range(16):
|
||||
key_state[i][1] = (msg.keys & (1 << i)) >> i
|
||||
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
|
||||
input("Press Enter to continue...")
|
||||
|
||||
print(key_state)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv)>1:
|
||||
ChannelFactoryInitialize(0, sys.argv[1])
|
||||
else:
|
||||
ChannelFactoryInitialize(0)
|
||||
|
||||
sub = ChannelSubscriber("rt/wirelesscontroller", WirelessController_)
|
||||
sub.Init(WirelessControllerHandler, 10)
|
||||
|
||||
while True:
|
||||
time.sleep(10.0)
|
||||
|
||||
custom = Custom()
|
||||
custom.Init()
|
||||
|
||||
while True:
|
||||
time.sleep(1)
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
[build-system]
|
||||
requires = ["setuptools", "wheel"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
11
setup.py
11
setup.py
|
|
@ -1,16 +1,21 @@
|
|||
from setuptools import setup, find_packages
|
||||
|
||||
setup(name='unitree_sdk2py',
|
||||
version='1.0.0',
|
||||
author='Unitree',
|
||||
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",
|
||||
],
|
||||
)
|
||||
)
|
||||
|
|
@ -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
|
||||
|
|
@ -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, [])
|
||||
|
|
@ -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
|
||||
|
|
@ -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, [])
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
"""
|
||||
" service name
|
||||
"""
|
||||
VIDEO_SERVICE_NAME = "videohub"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
VIDEO_API_VERSION = "1.0.0.1"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
VIDEO_API_ID_GETIMAGESAMPLE = 1001
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .video_api import *
|
||||
|
||||
|
||||
"""
|
||||
" class VideoClient
|
||||
"""
|
||||
class VideoClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(VIDEO_SERVICE_NAME, False)
|
||||
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(VIDEO_API_VERSION)
|
||||
# regist api
|
||||
self._RegistApi(VIDEO_API_ID_GETIMAGESAMPLE, 0)
|
||||
|
||||
# 1001
|
||||
def GetImageSample(self):
|
||||
return self._CallBinary(VIDEO_API_ID_GETIMAGESAMPLE, [])
|
||||
|
|
@ -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,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
|
||||
"""
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
"""
|
||||
" service name
|
||||
"""
|
||||
LOCO_SERVICE_NAME = "loco"
|
||||
|
||||
|
||||
"""
|
||||
" service api version
|
||||
"""
|
||||
LOCO_API_VERSION = "1.0.0.0"
|
||||
|
||||
|
||||
"""
|
||||
" api id
|
||||
"""
|
||||
ROBOT_API_ID_LOCO_GET_FSM_ID = 7001
|
||||
ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002
|
||||
ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 7003
|
||||
ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 7004
|
||||
ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 7005
|
||||
ROBOT_API_ID_LOCO_GET_PHASE = 7006 # deprecated
|
||||
|
||||
ROBOT_API_ID_LOCO_SET_FSM_ID = 7101
|
||||
ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102
|
||||
ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103
|
||||
ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104
|
||||
ROBOT_API_ID_LOCO_SET_VELOCITY = 7105
|
||||
ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106
|
||||
|
||||
"""
|
||||
" error code
|
||||
"""
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
import json
|
||||
|
||||
from ...rpc.client import Client
|
||||
from .g1_loco_api import *
|
||||
|
||||
"""
|
||||
" class SportClient
|
||||
"""
|
||||
class LocoClient(Client):
|
||||
def __init__(self):
|
||||
super().__init__(LOCO_SERVICE_NAME, False)
|
||||
self.first_shake_hand_stage_ = -1
|
||||
|
||||
def Init(self):
|
||||
# set api version
|
||||
self._SetApiVerson(LOCO_API_VERSION)
|
||||
|
||||
# regist api
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_ID, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_MODE, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_GET_PHASE, 0) # deprecated
|
||||
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_SET_FSM_ID, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_SET_VELOCITY, 0)
|
||||
self._RegistApi(ROBOT_API_ID_LOCO_SET_ARM_TASK, 0)
|
||||
|
||||
# 7101
|
||||
def SetFsmId(self, fsm_id: int):
|
||||
p = {}
|
||||
p["data"] = fsm_id
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter)
|
||||
return code
|
||||
|
||||
# 7102
|
||||
def SetBalanceMode(self, balance_mode: int):
|
||||
p = {}
|
||||
p["data"] = balance_mode
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, parameter)
|
||||
return code
|
||||
|
||||
# 7104
|
||||
def SetStandHeight(self, stand_height: float):
|
||||
p = {}
|
||||
p["data"] = stand_height
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter)
|
||||
return code
|
||||
|
||||
# 7105
|
||||
def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0):
|
||||
p = {}
|
||||
velocity = [vx,vy,omega]
|
||||
p["velocity"] = velocity
|
||||
p["duration"] = duration
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter)
|
||||
return code
|
||||
|
||||
# 7106
|
||||
def SetTaskId(self, task_id: float):
|
||||
p = {}
|
||||
p["data"] = task_id
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter)
|
||||
return code
|
||||
|
||||
def Damp(self):
|
||||
self.SetFsmId(1)
|
||||
|
||||
def Start(self):
|
||||
self.SetFsmId(200)
|
||||
|
||||
def Squat(self):
|
||||
self.SetFsmId(2)
|
||||
|
||||
def Sit(self):
|
||||
self.SetFsmId(3)
|
||||
|
||||
def StandUp(self):
|
||||
self.SetFsmId(4)
|
||||
|
||||
def ZeroTorque(self):
|
||||
self.SetFsmId(0)
|
||||
|
||||
def StopMove(self):
|
||||
self.SetVelocity(0., 0., 0.)
|
||||
|
||||
def HighStand(self):
|
||||
UINT32_MAX = (1 << 32) - 1
|
||||
self.SetStandHeight(UINT32_MAX)
|
||||
|
||||
def LowStand(self):
|
||||
UINT32_MIN = 0
|
||||
self.SetStandHeight(UINT32_MIN)
|
||||
|
||||
def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False):
|
||||
duration = 864000.0 if continous_move else 1
|
||||
self.SetVelocity(vx, vy, vyaw, duration)
|
||||
|
||||
def BalanceStand(self, balance_mode: int):
|
||||
self.SetBalanceMode(balance_mode)
|
||||
|
||||
def WaveHand(self, turn_flag: bool = False):
|
||||
self.SetTaskId(1 if turn_flag else 0)
|
||||
|
||||
def ShakeHand(self, stage: int = -1):
|
||||
if stage == 0:
|
||||
self.first_shake_hand_stage_ = False
|
||||
self.SetTaskId(2)
|
||||
elif stage == 1:
|
||||
self.first_shake_hand_stage_ = True
|
||||
self.SetTaskId(3)
|
||||
else:
|
||||
self.first_shake_hand_stage_ = not self.first_shake_hand_stage_
|
||||
return self.SetTaskId(3 if self.first_shake_hand_stage_ else 2)
|
||||
|
||||
|
|
@ -49,7 +49,20 @@ SPORT_API_ID_WIGGLEHIPS = 1033
|
|||
SPORT_API_ID_GETSTATE = 1034
|
||||
SPORT_API_ID_ECONOMICGAIT = 1035
|
||||
SPORT_API_ID_HEART = 1036
|
||||
ROBOT_SPORT_API_ID_DANCE3 = 1037
|
||||
ROBOT_SPORT_API_ID_DANCE4 = 1038
|
||||
ROBOT_SPORT_API_ID_HOPSPINLEFT = 1039
|
||||
ROBOT_SPORT_API_ID_HOPSPINRIGHT = 1040
|
||||
|
||||
ROBOT_SPORT_API_ID_LEFTFLIP = 1042
|
||||
ROBOT_SPORT_API_ID_BACKFLIP = 1044
|
||||
ROBOT_SPORT_API_ID_FREEWALK = 1045
|
||||
ROBOT_SPORT_API_ID_FREEBOUND = 1046
|
||||
ROBOT_SPORT_API_ID_FREEJUMP = 1047
|
||||
ROBOT_SPORT_API_ID_FREEAVOID = 1048
|
||||
ROBOT_SPORT_API_ID_WALKSTAIR = 1049
|
||||
ROBOT_SPORT_API_ID_WALKUPRIGHT = 1050
|
||||
ROBOT_SPORT_API_ID_CROSSSTEP = 1051
|
||||
|
||||
"""
|
||||
" error code
|
||||
|
|
|
|||
|
|
@ -73,6 +73,16 @@ class SportClient(Client):
|
|||
self._RegistApi(SPORT_API_ID_ECONOMICGAIT, 0)
|
||||
self._RegistApi(SPORT_API_ID_HEART, 0)
|
||||
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_LEFTFLIP, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_BACKFLIP, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_FREEWALK, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_FREEBOUND, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_FREEJUMP, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_FREEAVOID, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_WALKSTAIR, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_WALKUPRIGHT, 0)
|
||||
self._RegistApi(ROBOT_SPORT_API_ID_CROSSSTEP, 0)
|
||||
|
||||
# 1001
|
||||
def Damp(self):
|
||||
p = {}
|
||||
|
|
@ -361,4 +371,76 @@ class SportClient(Client):
|
|||
p = {}
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(SPORT_API_ID_HEART, parameter)
|
||||
return code
|
||||
|
||||
# 1042
|
||||
def LeftFlip(self):
|
||||
p = {}
|
||||
p["data"] = True
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_LEFTFLIP, parameter)
|
||||
return code
|
||||
|
||||
# 1044
|
||||
def BackFlip(self):
|
||||
p = {}
|
||||
p["data"] = True
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_BACKFLIP, parameter)
|
||||
return code
|
||||
|
||||
# 1045
|
||||
def FreeWalk(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = True
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_FREEWALK, parameter)
|
||||
return code
|
||||
|
||||
# 1046
|
||||
def FreeBound(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_FREEBOUND, parameter)
|
||||
return code
|
||||
|
||||
# 1047
|
||||
def FreeJump(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_FREEJUMP, parameter)
|
||||
return code
|
||||
|
||||
# 1048
|
||||
def FreeAvoid(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_FREEAVOID, parameter)
|
||||
return code
|
||||
|
||||
# 1049
|
||||
def WalkStair(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_WALKSTAIR, parameter)
|
||||
return code
|
||||
|
||||
# 1050
|
||||
def WalkUpright(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_WALKUPRIGHT, parameter)
|
||||
return code
|
||||
|
||||
# 1051
|
||||
def CrossStep(self, flag: bool):
|
||||
p = {}
|
||||
p["data"] = flag
|
||||
parameter = json.dumps(p)
|
||||
code, data = self._Call(ROBOT_SPORT_API_ID_CROSSSTEP, parameter)
|
||||
return code
|
||||
|
|
@ -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
|
||||
"""
|
||||
|
|
@ -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)
|
||||
|
|
@ -226,13 +226,16 @@ def unitree_hg_msg_dds__LowState_():
|
|||
|
||||
def unitree_hg_msg_dds__PressSensorState_():
|
||||
return HGPressSensorState_([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 0, 0)
|
||||
|
||||
def unitree_hg_msg_dds__HandCmd_():
|
||||
return HGHandCmd_([])
|
||||
return HGHandCmd_([unitree_hg_msg_dds__MotorCmd_() for i in range(7)], [0, 0, 0, 0])
|
||||
|
||||
def unitree_hg_msg_dds__HandState_():
|
||||
return HGHandState([], unitree_hg_msg_dds__IMUState_(), [], 0.0, 0.0, [0, 0])
|
||||
return HGHandState_([unitree_hg_msg_dds__MotorState_() for i in range(7)],
|
||||
[unitree_hg_msg_dds__PressSensorState_() for i in range(7)],
|
||||
unitree_hg_msg_dds__IMUState_(),
|
||||
0.0, 0.0, 0.0, 0.0, [0, 0], [0, 0])
|
||||
|
||||
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -0,0 +1,23 @@
|
|||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: unitree_go.msg.dds_
|
||||
IDL file: MotorCmds_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class MotorCmds_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorCmds_"):
|
||||
cmds: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorCmd_'] = field(default_factory=lambda: [])
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
"""
|
||||
Generated by Eclipse Cyclone DDS idlc Python Backend
|
||||
Cyclone DDS IDL version: v0.11.0
|
||||
Module: unitree_go.msg.dds_
|
||||
IDL file: MotorStates_.idl
|
||||
|
||||
"""
|
||||
|
||||
from enum import auto
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import cyclonedds.idl as idl
|
||||
import cyclonedds.idl.annotations as annotate
|
||||
import cyclonedds.idl.types as types
|
||||
|
||||
@dataclass
|
||||
@annotate.final
|
||||
@annotate.autoid("sequential")
|
||||
class MotorStates_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorStates_"):
|
||||
states: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorState_'] = field(default_factory=lambda: [])
|
||||
|
||||
|
||||
|
|
@ -17,7 +17,9 @@ from ._LidarState_ import LidarState_
|
|||
from ._LowCmd_ import LowCmd_
|
||||
from ._LowState_ import LowState_
|
||||
from ._MotorCmd_ import MotorCmd_
|
||||
from ._MotorCmds_ import MotorCmds_
|
||||
from ._MotorState_ import MotorState_
|
||||
from ._MotorStates_ import MotorStates_
|
||||
from ._Req_ import Req_
|
||||
from ._Res_ import Res_
|
||||
from ._SportModeState_ import SportModeState_
|
||||
|
|
@ -26,4 +28,4 @@ from ._PathPoint_ import PathPoint_
|
|||
from ._UwbState_ import UwbState_
|
||||
from ._UwbSwitch_ import UwbSwitch_
|
||||
from ._WirelessController_ import WirelessController_
|
||||
__all__ = ["AudioData_", "BmsCmd_", "BmsState_", "Error_", "Go2FrontVideoData_", "HeightMap_", "IMUState_", "InterfaceConfig_", "LidarState_", "LowCmd_", "LowState_", "MotorCmd_", "MotorState_", "Req_", "Res_", "SportModeState_", "TimeSpec_", "PathPoint_", "UwbState_", "UwbSwitch_", "WirelessController_", ]
|
||||
__all__ = ["AudioData_", "BmsCmd_", "BmsState_", "Error_", "Go2FrontVideoData_", "HeightMap_", "IMUState_", "InterfaceConfig_", "LidarState_", "LowCmd_", "LowState_", "MotorCmd_", "MotorCmds_", "MotorState_", "MotorStates_", "Req_", "Res_", "SportModeState_", "TimeSpec_", "PathPoint_", "UwbState_", "UwbSwitch_", "WirelessController_", ]
|
||||
|
|
|
|||
|
|
@ -23,5 +23,6 @@ import cyclonedds.idl.types as types
|
|||
@annotate.autoid("sequential")
|
||||
class HandCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.HandCmd_"):
|
||||
motor_cmd: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorCmd_']
|
||||
reserve: types.array[types.uint32, 4]
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -23,10 +23,13 @@ import cyclonedds.idl.types as types
|
|||
@annotate.autoid("sequential")
|
||||
class HandState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.HandState_"):
|
||||
motor_state: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorState_']
|
||||
imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_'
|
||||
press_sensor_state: types.sequence['unitree_sdk2py.idl.unitree_hg.msg.dds_.PressSensorState_']
|
||||
imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_'
|
||||
power_v: types.float32
|
||||
power_a: types.float32
|
||||
system_v: types.float32
|
||||
device_v: types.float32
|
||||
error: types.array[types.uint32, 2]
|
||||
reserve: types.array[types.uint32, 2]
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -24,5 +24,7 @@ import cyclonedds.idl.types as types
|
|||
class PressSensorState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.PressSensorState_"):
|
||||
pressure: types.array[types.float32, 12]
|
||||
temperature: types.array[types.float32, 12]
|
||||
lost: types.uint32
|
||||
reserve: types.uint32
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,251 @@
|
|||
import math
|
||||
import struct
|
||||
import os
|
||||
os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = "hide" # Disable pygame welcome message
|
||||
import pygame
|
||||
import time
|
||||
|
||||
class Button:
|
||||
def __init__(self) -> None:
|
||||
self.pressed = False
|
||||
self.on_pressed = False
|
||||
self.on_released = False
|
||||
self.data = 0
|
||||
self.click_count = 0 # 记录连续点击次数
|
||||
self.last_pressed_time = 0 # 上次按下时间
|
||||
|
||||
def __call__(self, data) -> None:
|
||||
current_time = time.perf_counter()
|
||||
# print('before',self.data)
|
||||
|
||||
self.pressed = (data != 0)
|
||||
self.on_pressed = self.pressed and self.data == 0
|
||||
self.on_released = not self.pressed and self.data != 0
|
||||
|
||||
# print('after',self.data)
|
||||
# 处理连续点击
|
||||
if self.on_pressed:
|
||||
# print('on_pressed')
|
||||
# print('on_pressed current_time',current_time)
|
||||
# print('on_pressed last_pressed_time',self.last_pressed_time)
|
||||
# print('on_pressed diff',current_time-self.last_pressed_time)
|
||||
|
||||
if current_time - self.last_pressed_time <= 0.3: # 0.1 秒以内的连续点击
|
||||
self.click_count += 1
|
||||
# print(self.click_count)
|
||||
else:
|
||||
self.click_count = 0 # 超过时间间隔，重置计数器
|
||||
self.last_pressed_time = current_time
|
||||
self.data = data
|
||||
|
||||
def reset_click_count(self):
|
||||
"""手动重置连续点击计数器"""
|
||||
self.click_count = 0
|
||||
|
||||
class Axis:
|
||||
def __init__(self) -> None:
|
||||
self.data = 0.0
|
||||
self.pressed = False
|
||||
self.on_pressed = False
|
||||
self.on_released = False
|
||||
|
||||
self.smooth = 0.03
|
||||
self.deadzone = 0.01
|
||||
self.threshold = 0.5
|
||||
|
||||
def __call__(self, data) -> None:
|
||||
data_deadzone = 0.0 if math.fabs(data) < self.deadzone else data
|
||||
new_data = self.data * (1 - self.smooth) + data_deadzone * self.smooth
|
||||
self.pressed = math.fabs(new_data) > self.threshold
|
||||
self.on_pressed = self.pressed and math.fabs(self.data) < self.threshold
|
||||
self.on_released = not self.pressed and math.fabs(self.data) > self.threshold
|
||||
self.data = new_data
|
||||
|
||||
|
||||
class Joystick:
|
||||
def __init__(self) -> None:
|
||||
# Buttons
|
||||
self.back = Button()
|
||||
self.start = Button()
|
||||
# self.LS = Button()
|
||||
# self.RS = Button()
|
||||
self.LB = Button()
|
||||
self.RB = Button()
|
||||
self.LT = Button()
|
||||
self.RT = Button()
|
||||
self.A = Button()
|
||||
self.B = Button()
|
||||
self.X = Button()
|
||||
self.Y = Button()
|
||||
self.up = Button()
|
||||
self.down = Button()
|
||||
self.left = Button()
|
||||
self.right = Button()
|
||||
self.F1 = Button()
|
||||
self.F2 = Button()
|
||||
|
||||
# Axes
|
||||
# self.LT = Axis()
|
||||
# self.RT = Axis()
|
||||
self.lx = Axis()
|
||||
self.ly = Axis()
|
||||
self.rx = Axis()
|
||||
self.ry = Axis()
|
||||
|
||||
self.last_active_time = time.perf_counter() # 最后一次活动时间
|
||||
self.inactive_timeout = 0.5 # 超时时间（单位：秒）
|
||||
def update(self):
|
||||
"""
|
||||
Update the current handle key based on the original data
|
||||
Used to update flag bits such as on_pressed
|
||||
|
||||
Examples:
|
||||
>>> new_A_data = 1
|
||||
>>> self.A( new_A_data )
|
||||
"""
|
||||
pass
|
||||
|
||||
def extract(self, wireless_remote):
|
||||
"""
|
||||
Extract data from unitree_joystick
|
||||
wireless_remote: uint8_t[40]
|
||||
"""
|
||||
# Buttons
|
||||
button1 = [int(data) for data in f'{wireless_remote[2]:08b}']
|
||||
button2 = [int(data) for data in f'{wireless_remote[3]:08b}']
|
||||
self.LT(button1[2])
|
||||
self.RT(button1[3])
|
||||
self.back(button1[4])
|
||||
self.start(button1[5])
|
||||
self.LB(button1[6])
|
||||
self.RB(button1[7])
|
||||
self.left(button2[0])
|
||||
self.down(button2[1])
|
||||
self.right(button2[2])
|
||||
self.up(button2[3])
|
||||
self.Y(button2[4])
|
||||
self.X(button2[5])
|
||||
self.B(button2[6])
|
||||
self.A(button2[7])
|
||||
# Axes
|
||||
self.lx( struct.unpack('f', bytes(wireless_remote[4:8]))[0] )
|
||||
self.rx( struct.unpack('f', bytes(wireless_remote[8:12]))[0] )
|
||||
self.ry( struct.unpack('f', bytes(wireless_remote[12:16]))[0] )
|
||||
self.ly( struct.unpack('f', bytes(wireless_remote[20:24]))[0] )
|
||||
|
||||
|
||||
# 检查是否有按键按下
|
||||
if any([
|
||||
self.LT.pressed, self.RT.pressed, self.back.pressed, self.start.pressed,
|
||||
self.LB.pressed, self.RB.pressed, self.left.pressed, self.down.pressed,
|
||||
self.right.pressed, self.up.pressed, self.Y.pressed, self.X.pressed,
|
||||
self.B.pressed, self.A.pressed
|
||||
]):
|
||||
self.last_active_time = time.perf_counter() # 更新最后一次活动时间
|
||||
elif time.perf_counter() - self.last_active_time > self.inactive_timeout:
|
||||
# 超过设定的超时时间未按下任何键，重置所有按键的点击计数
|
||||
self.reset_all_click_counts()
|
||||
self.last_active_time = time.perf_counter() # 重置最后活动时间
|
||||
|
||||
def reset_all_click_counts(self):
|
||||
"""重置所有按键的连续点击计数器"""
|
||||
for button in [
|
||||
self.LT, self.RT, self.back, self.start, self.LB, self.RB,
|
||||
self.left, self.down, self.right, self.up, self.Y, self.X, self.B, self.A
|
||||
]:
|
||||
button.reset_click_count()
|
||||
|
||||
def combine(self):
|
||||
"""
|
||||
Merge data from Joystick to wireless_remote
|
||||
"""
|
||||
# prepare an empty list
|
||||
wireless_remote = [0 for _ in range(40)]
|
||||
|
||||
# Buttons
|
||||
wireless_remote[2] = int(''.join([f'{key}' for key in [
|
||||
0, 0, round(self.LT.data), round(self.RT.data),
|
||||
self.back.data, self.start.data, self.LB.data, self.RB.data,
|
||||
]]), 2)
|
||||
wireless_remote[3] = int(''.join([f'{key}' for key in [
|
||||
self.left.data, self.down.data, self.right.data,
|
||||
self.up.data, self.Y.data, self.X.data, self.B.data, self.A.data,
|
||||
]]), 2)
|
||||
|
||||
# Axes
|
||||
sticks = [self.lx.data, self.rx.data, self.ry.data, self.ly.data]
|
||||
packs = list(map(lambda x: struct.pack('f', x), sticks))
|
||||
wireless_remote[4:8] = packs[0]
|
||||
wireless_remote[8:12] = packs[1]
|
||||
wireless_remote[12:16] = packs[2]
|
||||
wireless_remote[20:24] = packs[3]
|
||||
return wireless_remote
|
||||
|
||||
class PyGameJoystick(Joystick):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
pygame.init()
|
||||
pygame.joystick.init()
|
||||
if pygame.joystick.get_count() <= 0:
|
||||
raise Exception("No joystick found!")
|
||||
|
||||
self._joystick = pygame.joystick.Joystick(0)
|
||||
self._joystick.init()
|
||||
|
||||
def print(self):
|
||||
print("\naxes: ")
|
||||
for i in range(self._joystick.get_numaxes()):
|
||||
print(self._joystick.get_axis(i), end=" ")
|
||||
print("\nbuttons: ")
|
||||
for i in range(self._joystick.get_numbuttons()):
|
||||
print(self._joystick.get_button(i), end=" ")
|
||||
print("\nhats: ")
|
||||
for i in range(self._joystick.get_numhats()):
|
||||
print(self._joystick.get_hat(i), end=" ")
|
||||
print("\nballs: ")
|
||||
for i in range(self._joystick.get_numballs()):
|
||||
print(self._joystick.get_ball(i), end=" ")
|
||||
print("\n")
|
||||
|
||||
class LogicJoystick(PyGameJoystick):
|
||||
""" Logic F710 """
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
def update(self):
|
||||
pygame.event.pump()
|
||||
|
||||
self.back(self._joystick.get_button(6))
|
||||
self.start(self._joystick.get_button(7))
|
||||
self.LS(self._joystick.get_button(9))
|
||||
self.RS(self._joystick.get_button(10))
|
||||
self.LB(self._joystick.get_button(4))
|
||||
self.RB(self._joystick.get_button(5))
|
||||
self.A(self._joystick.get_button(0))
|
||||
self.B(self._joystick.get_button(1))
|
||||
self.X(self._joystick.get_button(2))
|
||||
self.Y(self._joystick.get_button(3))
|
||||
|
||||
self.LT((self._joystick.get_axis(2) + 1)/2)
|
||||
self.RT((self._joystick.get_axis(5) + 1)/2)
|
||||
self.rx(self._joystick.get_axis(3))
|
||||
self.ry(-self._joystick.get_axis(4))
|
||||
|
||||
|
||||
# Logitech controller has 2 modes
|
||||
# mode 1: light down
|
||||
self.up(1 if self._joystick.get_hat(0)[1] > 0.5 else 0)
|
||||
self.down(1 if self._joystick.get_hat(0)[1] < -0.5 else 0)
|
||||
self.left(1 if self._joystick.get_hat(0)[0] < -0.5 else 0)
|
||||
self.right(1 if self._joystick.get_hat(0)[0] > 0.5 else 0)
|
||||
self.lx(self._joystick.get_axis(0))
|
||||
self.ly(-self._joystick.get_axis(1))
|
||||
# mode 2: light up
|
||||
# self.up(1 if self._joystick.get_axis(1) < -0.5 else 0)
|
||||
# self.down(1 if self._joystick.get_axis(0) > 0.5 else 0)
|
||||
# self.left(1 if self._joystick.get_axis(0) < -0.5 else 0)
|
||||
# self.right(1 if self._joystick.get_axis(0) > 0.5 else 0)
|
||||
# self.lx(self._joystick.get_hat(0)[1])
|
||||
# self.ly(self._joystick.get_hat(0)[1])
|
||||
|
||||
Loading…
Reference in New Issue