51 lines
1.3 KiB
Python
51 lines
1.3 KiB
Python
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()
|