EmbodiedAI
/
unitree_sdk2_python
mirror of https://github.com/unitreerobotics/unitree_sdk2_python.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add unitree_hg

This commit is contained in:
yangning wu 2024-09-05 12:05:49 +08:00
parent f5bfa089b0
commit bddc17adeb
34 changed files with 860 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
example/hg/arm_sdk.py Normal file
View File

@ -0,0 +1,133 @@
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()

50
example/hg/low_cmd.py Normal file
View File

@ -0,0 +1,50 @@
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()

5
example/hg/readme.md Normal file
View File

@ -0,0 +1,5 @@
This example is a test of Unitree G1/H1-2 robot.
**Note:**
idl/unitree_go is used for Unitree Go2/B2/H1/B2w/Go2w robots
idl/unitree_hg is used for Unitree G1/H1-2 robots

2
setup.py
View File

@ -5,7 +5,7 @@ setup(name='unitree_sdk2py',
author='Unitree',
author_email='unitree@unitree.com',
license="BSD-3-Clause",
packages=find_packages(),
packages=find_packages(include=['unitree_sdk2py','unitree_sdk2py.*']),
description='Unitree robot sdk version 2 for python',
python_requires='>=3.8',
install_requires=[

0
unitree_sdk2py/core/__init__.py Normal file
View File

0
unitree_sdk2py/go2/__init__.py Normal file
View File

0
unitree_sdk2py/go2/obstacles_avoid/__init__.py Normal file
View File

0
unitree_sdk2py/go2/robot_state/__init__.py Normal file
View File

0
unitree_sdk2py/go2/sport/__init__.py Normal file
View File

0
unitree_sdk2py/go2/video/__init__.py Normal file
View File

0
unitree_sdk2py/go2/vui/__init__.py Normal file
View File

2
unitree_sdk2py/idl/__init__.py
View File

@ -1,3 +1,4 @@
from .default import *
from . import builtin_interfaces, geometry_msgs, sensor_msgs, std_msgs, unitree_go, unitree_api
__all__ = [
@ -6,5 +7,6 @@ __all__ = [
"sensor_msgs",
"std_msgs",
"unitree_go",
"unitree_hg",
"unitree_api",
]

74
unitree_sdk2py/idl/default.py
View File

@ -6,6 +6,17 @@ from .sensor_msgs.msg.dds_ import *
from .unitree_go.msg.dds_ import *
from .unitree_api.msg.dds_ import *
# IDL for unitree_hg
from .unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_
from .unitree_hg.msg.dds_ import LowState_ as HGLowState_
from .unitree_hg.msg.dds_ import MotorCmd_ as HGMotorCmd_
from .unitree_hg.msg.dds_ import MotorState_ as HGMotorState_
from .unitree_hg.msg.dds_ import BmsState_ as HGBmsState_
from .unitree_hg.msg.dds_ import IMUState_ as HGIMUState_
from .unitree_hg.msg.dds_ import MainBoardState_ as HGMainBoardState_
from .unitree_hg.msg.dds_ import PressSensorState_ as HGPressSensorState_
from .unitree_hg.msg.dds_ import HandCmd_ as HGHandCmd_
from .unitree_hg.msg.dds_ import HandState_ as HGHandState_
"""
" builtin_interfaces_msgs.msg.dds_ dafault
@ -89,7 +100,8 @@ def nav_msgs_msg_dds__OccupancyGrid_():
return OccupancyGrid_(std_msgs_msg_dds__Header_(), nav_msgs_msg_dds__MapMetaData_(), [])
def nav_msgs_msg_dds__Odometry_():
return Odometry_(std_msgs_msg_dds__Header_(), "", geometry_msgs_msg_dds__PoseWithCovariance_(), geometry_msgs_msg_dds__TwistWithCovariance_())
return Odometry_(std_msgs_msg_dds__Header_(), "", geometry_msgs_msg_dds__PoseWithCovariance_(),
geometry_msgs_msg_dds__TwistWithCovariance_())
"""
@ -139,12 +151,14 @@ def unitree_go_msg_dds__MotorState_():
return MotorState_(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, [0, 0])
def unitree_go_msg_dds__LowCmd_():
return LowCmd_([0, 0], 0, 0, [0, 0], [0, 0], 0, [unitree_go_msg_dds__MotorCmd_() for i in range(20)], unitree_go_msg_dds__BmsCmd_(),
return LowCmd_([0, 0], 0, 0, [0, 0], [0, 0], 0, [unitree_go_msg_dds__MotorCmd_() for i in range(20)],
unitree_go_msg_dds__BmsCmd_(),
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0], 0, 0, 0)
def unitree_go_msg_dds__LowState_():
return LowState_([0, 0], 0, 0, [0, 0], [0, 0], 0, unitree_go_msg_dds__IMUState_(), [unitree_go_msg_dds__MotorState_() for i in range(20)],
return LowState_([0, 0], 0, 0, [0, 0], [0, 0], 0, unitree_go_msg_dds__IMUState_(),
[unitree_go_msg_dds__MotorState_() for i in range(20)],
unitree_go_msg_dds__BmsState_(), [0, 0, 0, 0], [0, 0, 0, 0], 0,
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
0, 0, 0, 0, 0.0, 0.0, [0, 0, 0, 0], 0, 0)
@ -162,7 +176,8 @@ def unitree_go_msg_dds__PathPoint_():
return PathPoint_(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def unitree_go_msg_dds__SportModeState_():
return SportModeState_(unitree_go_msg_dds__TimeSpec_(), 0, unitree_go_msg_dds__IMUState_(), 0, 0, 0, 0.0, [0.0, 0.0, 0.0], 0.0,
return SportModeState_(unitree_go_msg_dds__TimeSpec_(), 0, unitree_go_msg_dds__IMUState_(),
0, 0, 0, 0.0, [0.0, 0.0, 0.0], 0.0,
[0.0, 0.0, 0.0], 0.0, [0.0, 0.0, 0.0, 0.0], [0, 0, 0, 0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],[unitree_go_msg_dds__PathPoint_() for i in range(10)])
@ -176,6 +191,50 @@ def unitree_go_msg_dds__WirelessController_():
return WirelessController_(0.0, 0.0, 0.0, 0.0, 0)
"""
" unitree_hg.msg.dds_ dafault
"""
def unitree_hg_msg_dds__BmsCmd_():
return HGBmsCmd_(0, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
def unitree_hg_msg_dds__BmsState_():
return HGBmsState_(0, 0, 0,
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0], 0, 0, 0, [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 0, 0, [0, 0, 0, 0, 0], [0, 0, 0])
def unitree_hg_msg_dds__IMUState_():
return HGIMUState_([0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], 0)
def unitree_hg_msg_dds__MotorCmd_():
return HGMotorCmd_(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0)
def unitree_hg_msg_dds__MotorState_():
return HGMotorState_(0, 0.0, 0.0, 0.0, 0.0, [0, 0], 0.0, [0, 0], 0, [0, 0, 0, 0])
def unitree_hg_msg_dds__MainBoardState_():
return HGMainBoardState_([0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0, 0, 0, 0, 0, 0])
def unitree_hg_msg_dds__LowCmd_():
return HGLowCmd_(0, 0, [unitree_hg_msg_dds__MotorCmd_() for i in range(35)], [0, 0, 0, 0], 0)
def unitree_hg_msg_dds__LowState_():
return HGLowState_([0, 0], 0, 0, 0, unitree_hg_msg_dds__IMUState_(),
[unitree_hg_msg_dds__MotorState_() for i in range(35)],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0], 0)
def unitree_hg_msg_dds__PressSensorState_():
return HGPressSensorState_([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
def unitree_hg_msg_dds__HandCmd_():
return HGHandCmd_([])
def unitree_hg_msg_dds__HandState_():
return HGHandState([], unitree_hg_msg_dds__IMUState_(), [], 0.0, 0.0, [0, 0])
"""
" unitree_api.msg.dds_ dafault
"""
@ -183,13 +242,14 @@ def unitree_api_msg_dds__RequestIdentity_():
return RequestIdentity_(0, 0)
def unitree_api_msg_dds__RequestLease_():
return RequestLease_(0)
return RequestLease_(0, unitree_hg_msg_dds__IMUState_(), [], )
def unitree_api_msg_dds__RequestPolicy_():
return RequestPolicy_(0, False)
def unitree_api_msg_dds__RequestHeader_():
return RequestHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__RequestLease_(), unitree_api_msg_dds__RequestPolicy_())
return RequestHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__RequestLease_(),
unitree_api_msg_dds__RequestPolicy_())
def unitree_api_msg_dds__Request_():
return Request_(unitree_api_msg_dds__RequestHeader_(), "", [])
@ -201,5 +261,5 @@ def unitree_api_msg_dds__ResponseHeader_():
return ResponseHeader_(unitree_api_msg_dds__RequestIdentity_(), unitree_api_msg_dds__ResponseStatus_())
def unitree_api_msg_dds__Response_():
return Response_(unitree_api_msg_dds__ResponseHeader_(), "", [])
return Response_(unitree_api_msg_dds__ResponseHeader_(), "", [], 0, 0, [0, 0])

43
unitree_sdk2py/idl/unitree_hg/.idlpy_manifest Normal file
View File

@ -0,0 +1,43 @@
BmsCmd_
msg
BmsState_
msg
HandCmd_
msg
HandState_
msg
IMUState_
msg
LowCmd_
msg
LowState_
msg
MainBoardState_
msg
MotorCmd_
msg
MotorState_
msg
PressSensorState_
msg

9
unitree_sdk2py/idl/unitree_hg/__init__.py Normal file
View File

@ -0,0 +1,9 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg
"""
from . import msg
__all__ = ["msg", ]

43
unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest Normal file
View File

@ -0,0 +1,43 @@
BmsCmd_
dds_
BmsState_
dds_
HandCmd_
dds_
HandState_
dds_
IMUState_
dds_
LowCmd_
dds_
LowState_
dds_
MainBoardState_
dds_
MotorCmd_
dds_
MotorState_
dds_
PressSensorState_
dds_

9
unitree_sdk2py/idl/unitree_hg/msg/__init__.py Normal file
View File

@ -0,0 +1,9 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg
"""
from . import dds_
__all__ = ["dds_", ]

43
unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest Normal file
View File

@ -0,0 +1,43 @@
BmsCmd_
BmsCmd_
BmsState_
BmsState_
HandCmd_
HandCmd_
HandState_
HandState_
IMUState_
IMUState_
LowCmd_
LowCmd_
LowState_
LowState_
MainBoardState_
MainBoardState_
MotorCmd_
MotorCmd_
MotorState_
MotorState_
PressSensorState_
PressSensorState_

28
unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py Normal file
View File

@ -0,0 +1,28 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: BmsCmd_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class BmsCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.BmsCmd_"):
cmd: types.uint8
reserve: types.array[types.uint8, 40]

39
unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py Normal file
View File

@ -0,0 +1,39 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: BmsState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class BmsState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.BmsState_"):
version_high: types.uint8
version_low: types.uint8
fn: types.uint8
cell_vol: types.array[types.uint16, 40]
bmsvoltage: types.array[types.uint32, 3]
current: types.int32
soc: types.uint8
soh: types.uint8
temperature: types.array[types.int16, 12]
cycle: types.uint16
manufacturer_date: types.uint16
bmsstate: types.array[types.uint32, 5]
reserve: types.array[types.uint32, 3]

27
unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py Normal file
View File

@ -0,0 +1,27 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: HandCmd_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@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_']

32
unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py Normal file
View File

@ -0,0 +1,32 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: HandState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@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_']
power_v: types.float32
power_a: types.float32
reserve: types.array[types.uint32, 2]

31
unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py Normal file
View File

@ -0,0 +1,31 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: IMUState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class IMUState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.IMUState_"):
quaternion: types.array[types.float32, 4]
gyroscope: types.array[types.float32, 3]
accelerometer: types.array[types.float32, 3]
rpy: types.array[types.float32, 3]
temperature: types.int16

31
unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py Normal file
View File

@ -0,0 +1,31 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: LowCmd_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class LowCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.LowCmd_"):
mode_pr: types.uint8
mode_machine: types.uint8
motor_cmd: types.array['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorCmd_', 35]
reserve: types.array[types.uint32, 4]
crc: types.uint32

35
unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py Normal file
View File

@ -0,0 +1,35 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: LowState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class LowState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.LowState_"):
version: types.array[types.uint32, 2]
mode_pr: types.uint8
mode_machine: types.uint8
tick: types.uint32
imu_state: 'unitree_sdk2py.idl.unitree_hg.msg.dds_.IMUState_'
motor_state: types.array['unitree_sdk2py.idl.unitree_hg.msg.dds_.MotorState_', 35]
wireless_remote: types.array[types.uint8, 40]
reserve: types.array[types.uint32, 4]
crc: types.uint32

30
unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py Normal file
View File

@ -0,0 +1,30 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: MainBoardState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class MainBoardState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.MainBoardState_"):
fan_state: types.array[types.uint16, 6]
temperature: types.array[types.int16, 6]
value: types.array[types.float32, 6]
state: types.array[types.uint32, 6]

33
unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py Normal file
View File

@ -0,0 +1,33 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: MotorCmd_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class MotorCmd_(idl.IdlStruct, typename="unitree_hg.msg.dds_.MotorCmd_"):
mode: types.uint8
q: types.float32
dq: types.float32
tau: types.float32
kp: types.float32
kd: types.float32
reserve: types.uint32

36
unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py Normal file
View File

@ -0,0 +1,36 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: MotorState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class MotorState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.MotorState_"):
mode: types.uint8
q: types.float32
dq: types.float32
ddq: types.float32
tau_est: types.float32
temperature: types.array[types.int16, 2]
vol: types.float32
sensor: types.array[types.uint32, 2]
motorstate: types.uint32
reserve: types.array[types.uint32, 4]

28
unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py Normal file
View File

@ -0,0 +1,28 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
IDL file: PressSensorState_.idl
"""
from enum import auto
from typing import TYPE_CHECKING, Optional
from dataclasses import dataclass
import cyclonedds.idl as idl
import cyclonedds.idl.annotations as annotate
import cyclonedds.idl.types as types
# root module import for resolving types
# import unitree_hg
@dataclass
@annotate.final
@annotate.autoid("sequential")
class PressSensorState_(idl.IdlStruct, typename="unitree_hg.msg.dds_.PressSensorState_"):
pressure: types.array[types.float32, 12]
temperature: types.array[types.float32, 12]

19
unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py Normal file
View File

@ -0,0 +1,19 @@
"""
Generated by Eclipse Cyclone DDS idlc Python Backend
Cyclone DDS IDL version: v0.11.0
Module: unitree_hg.msg.dds_
"""
from ._BmsCmd_ import BmsCmd_
from ._BmsState_ import BmsState_
from ._HandCmd_ import HandCmd_
from ._HandState_ import HandState_
from ._IMUState_ import IMUState_
from ._LowCmd_ import LowCmd_
from ._LowState_ import LowState_
from ._MainBoardState_ import MainBoardState_
from ._MotorCmd_ import MotorCmd_
from ._MotorState_ import MotorState_
from ._PressSensorState_ import PressSensorState_
__all__ = ["BmsCmd_", "BmsState_", "HandCmd_", "HandState_", "IMUState_", "LowCmd_", "LowState_", "MainBoardState_", "MotorCmd_", "MotorState_", "PressSensorState_", ]

0
unitree_sdk2py/rpc/__init__.py Normal file
View File

16
unitree_sdk2py/test/crc/test_crc.py
View File

@ -1,10 +1,11 @@
from unitree_sdk2py.idl.default import *
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_, unitree_go_msg_dds__LowState_
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_
from unitree_sdk2py.utils.crc import CRC
crc = CRC()
"""
" LowCmd CRC
" LowCmd/LowState CRC
"""
cmd = unitree_go_msg_dds__LowCmd_()
cmd.crc = crc.Crc(cmd)
@ -13,3 +14,14 @@ state = unitree_go_msg_dds__LowState_()
state.crc = crc.Crc(state)
print("CRC[LowCmd, LowState]: {}, {}".format(cmd.crc, state.crc))
"""
" LowCmd/LowState for HG CRC. ()
"""
cmd = unitree_hg_msg_dds__LowCmd_()
cmd.crc = crc.Crc(cmd)
state = unitree_hg_msg_dds__LowState_()
state.crc = crc.Crc(state)
print("CRC[HGLowCmd, HGLowState]: {}, {}".format(cmd.crc, state.crc))

0
unitree_sdk2py/utils/__init__.py Normal file
View File

69
unitree_sdk2py/utils/crc.py
View File

@ -3,8 +3,11 @@ import cyclonedds
import cyclonedds.idl as idl
from .singleton import Singleton
from ..idl.unitree_go.msg.dds_ import LowCmd_ as LowCmd
from ..idl.unitree_go.msg.dds_ import LowState_ as LowState
from ..idl.unitree_go.msg.dds_ import LowCmd_
from ..idl.unitree_go.msg.dds_ import LowState_
from ..idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_
from ..idl.unitree_hg.msg.dds_ import LowState_ as HGLowState_
class CRC(Singleton):
def __init__(self):
@ -13,16 +16,24 @@ class CRC(Singleton):
self.__packFmtLowCmd = '<4B4IH2x' + 'B3x5f3I' * 20 + '4B' + '55Bx2I'
#size 1180
self.__packFmtLowState = '<4B4IH2x' + '13fb3x' + 'B3x7fb3x3I' * 20 + '4BiH4b15H' + '8hI41B3xf2b2x2f4h2I'
#size 1004
self.__packFmtHGLowCmd = '<2B2x' + 'B3x5fI' * 35 + '5I'
#size 2092
self.__packFmtHGLowState = '<2I2B2xI' + '13fh2x' + 'B3x4f2hf7I' * 35 + '40B5I'
def Crc(self, msg: idl.IdlStruct):
if msg.__idl_typename__ == 'unitree_go.msg.dds_.LowCmd_':
return self.__Crc32(self.__PackLowCmd(msg))
elif msg.__idl_typename__ == 'unitree_go.msg.dds_.LowState_':
return self.__Crc32(self.__PackLowState(msg))
if msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowCmd_':
return self.__Crc32(self.__PackHGLowCmd(msg))
elif msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowState_':
return self.__Crc32(self.__PackHGLowState(msg))
else:
raise TypeError('unknown IDL message type to crc')
def __PackLowCmd(self, cmd: LowCmd):
def __PackLowCmd(self, cmd: LowCmd_):
origData = []
origData.extend(cmd.head)
origData.append(cmd.level_flag)
@ -52,7 +63,7 @@ class CRC(Singleton):
return self.__Trans(struct.pack(self.__packFmtLowCmd, *origData))
def __PackLowState(self, state: LowState):
def __PackLowState(self, state: LowState_):
origData = []
origData.extend(state.head)
origData.append(state.level_flag)
@ -106,6 +117,56 @@ class CRC(Singleton):
return self.__Trans(struct.pack(self.__packFmtLowState, *origData))
def __PackHGLowCmd(self, cmd: HGLowCmd_):
origData = []
origData.append(cmd.mode_pr)
origData.append(cmd.mode_machine)
for i in range(35):
origData.append(cmd.motor_cmd[i].mode)
origData.append(cmd.motor_cmd[i].q)
origData.append(cmd.motor_cmd[i].dq)
origData.append(cmd.motor_cmd[i].tau)
origData.append(cmd.motor_cmd[i].kp)
origData.append(cmd.motor_cmd[i].kd)
origData.append(cmd.motor_cmd[i].reserve)
origData.extend(cmd.reserve)
origData.append(cmd.crc)
return self.__Trans(struct.pack(self.__packFmtHGLowCmd, *origData))
def __PackHGLowState(self, state: HGLowState_):
origData = []
origData.extend(state.version)
origData.append(state.mode_pr)
origData.append(state.mode_machine)
origData.append(state.tick)
origData.extend(state.imu_state.quaternion)
origData.extend(state.imu_state.gyroscope)
origData.extend(state.imu_state.accelerometer)
origData.extend(state.imu_state.rpy)
origData.append(state.imu_state.temperature)
for i in range(35):
origData.append(state.motor_state[i].mode)
origData.append(state.motor_state[i].q)
origData.append(state.motor_state[i].dq)
origData.append(state.motor_state[i].ddq)
origData.append(state.motor_state[i].tau_est)
origData.extend(state.motor_state[i].temperature)
origData.append(state.motor_state[i].vol)
origData.extend(state.motor_state[i].sensor)
origData.append(state.motor_state[i].motorstate)
origData.extend(state.motor_state[i].reserve)
origData.extend(state.wireless_remote)
origData.extend(state.reserve)
origData.append(state.crc)
return self.__Trans(struct.pack(self.__packFmtHGLowState, *origData))
def __Trans(self, packData):
calcData = []
calcLen = ((len(packData)>>2)-1)