diff --git a/example/hg/arm_sdk.py b/example/hg/arm_sdk.py new file mode 100644 index 0000000..ff6d6b4 --- /dev/null +++ b/example/hg/arm_sdk.py @@ -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() \ No newline at end of file diff --git a/example/hg/low_cmd.py b/example/hg/low_cmd.py new file mode 100644 index 0000000..331bfe5 --- /dev/null +++ b/example/hg/low_cmd.py @@ -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() diff --git a/example/hg/readme.md b/example/hg/readme.md new file mode 100644 index 0000000..9fb7900 --- /dev/null +++ b/example/hg/readme.md @@ -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 diff --git a/setup.py b/setup.py index b69821b..3c774e0 100644 --- a/setup.py +++ b/setup.py @@ -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=[ diff --git a/unitree_sdk2py/core/__init__.py b/unitree_sdk2py/core/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/go2/__init__.py b/unitree_sdk2py/go2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/go2/obstacles_avoid/__init__.py b/unitree_sdk2py/go2/obstacles_avoid/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/go2/robot_state/__init__.py b/unitree_sdk2py/go2/robot_state/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/go2/sport/__init__.py b/unitree_sdk2py/go2/sport/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/go2/video/__init__.py b/unitree_sdk2py/go2/video/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/go2/vui/__init__.py b/unitree_sdk2py/go2/vui/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/idl/__init__.py b/unitree_sdk2py/idl/__init__.py index d11617e..38d706e 100644 --- a/unitree_sdk2py/idl/__init__.py +++ b/unitree_sdk2py/idl/__init__.py @@ -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", ] diff --git a/unitree_sdk2py/idl/default.py b/unitree_sdk2py/idl/default.py index 821a3a3..f21210f 100644 --- a/unitree_sdk2py/idl/default.py +++ b/unitree_sdk2py/idl/default.py @@ -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,15 +151,17 @@ 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_(), - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0], 0, 0, 0) + 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)], - 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) + 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) def unitree_go_msg_dds__Req_(): return Req_("", "") @@ -162,9 +176,10 @@ 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, - [0.0, 0.0, 0.0], 0.0, [0.0, 0.0, 0.0, 0.0], [0, 0, 0, 0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [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)]) + 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)]) def unitree_go_msg_dds__UwbState_(): return UwbState_([0, 0], 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, [0.0, 0.0], 0, 0, 0) @@ -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]) diff --git a/unitree_sdk2py/idl/unitree_hg/.idlpy_manifest b/unitree_sdk2py/idl/unitree_hg/.idlpy_manifest new file mode 100644 index 0000000..fd4ea94 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/.idlpy_manifest @@ -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 + diff --git a/unitree_sdk2py/idl/unitree_hg/__init__.py b/unitree_sdk2py/idl/unitree_hg/__init__.py new file mode 100644 index 0000000..5f55ee5 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/__init__.py @@ -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", ] diff --git a/unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest b/unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest new file mode 100644 index 0000000..3218e96 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/.idlpy_manifest @@ -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_ + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/__init__.py b/unitree_sdk2py/idl/unitree_hg/msg/__init__.py new file mode 100644 index 0000000..02b132d --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/__init__.py @@ -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_", ] diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest b/unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest new file mode 100644 index 0000000..6174f37 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/.idlpy_manifest @@ -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_ diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py new file mode 100644 index 0000000..525d1a4 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsCmd_.py @@ -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] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py new file mode 100644 index 0000000..dd0c6bd --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_BmsState_.py @@ -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] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py new file mode 100644 index 0000000..a8c74e6 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py @@ -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_'] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py new file mode 100644 index 0000000..b4e65d6 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py @@ -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] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py new file mode 100644 index 0000000..75d01a6 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_IMUState_.py @@ -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 + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py new file mode 100644 index 0000000..56b4b8b --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowCmd_.py @@ -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 + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py new file mode 100644 index 0000000..8e95ac7 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_LowState_.py @@ -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 + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py new file mode 100644 index 0000000..c280245 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MainBoardState_.py @@ -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] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py new file mode 100644 index 0000000..3dd8f9d --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorCmd_.py @@ -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 + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py new file mode 100644 index 0000000..839fccf --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_MotorState_.py @@ -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] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py new file mode 100644 index 0000000..d260568 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py @@ -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] + + diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py new file mode 100644 index 0000000..87b865f --- /dev/null +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/__init__.py @@ -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_", ] diff --git a/unitree_sdk2py/rpc/__init__.py b/unitree_sdk2py/rpc/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/test/crc/test_crc.py b/unitree_sdk2py/test/crc/test_crc.py index 67d2b7d..ceb1b4b 100644 --- a/unitree_sdk2py/test/crc/test_crc.py +++ b/unitree_sdk2py/test/crc/test_crc.py @@ -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)) diff --git a/unitree_sdk2py/utils/__init__.py b/unitree_sdk2py/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/utils/crc.py b/unitree_sdk2py/utils/crc.py index d7dc44f..3ab5650 100644 --- a/unitree_sdk2py/utils/crc.py +++ b/unitree_sdk2py/utils/crc.py @@ -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)