diff --git a/.conda-env b/.conda-env new file mode 100644 index 00000000..732098d1 --- /dev/null +++ b/.conda-env @@ -0,0 +1 @@ +ros diff --git a/example/wireless_controller/wireless_controller_g1.py b/example/wireless_controller/wireless_controller_g1.py new file mode 100644 index 00000000..0f55408c --- /dev/null +++ b/example/wireless_controller/wireless_controller_g1.py @@ -0,0 +1,62 @@ +import time +import sys +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize + +from unitree_sdk2py.idl.default import unitree_go_msg_dds__WirelessController_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_ + + +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 encode_keys(keys): + key_map = {key[0]: i for i, key in enumerate(key_state)} + encoded_value = 0 + + for key in keys: + if key in key_map: + encoded_value |= 1 << key_map[key] + else: + print(f"Warning: {key} is not a valid key") + + return encoded_value + + +if __name__ == "__main__": + ChannelFactoryInitialize(0, sys.argv[1]) + + pub = ChannelPublisher("rt/wirelesscontroller", WirelessController_) + pub.Init() + smp = unitree_go_msg_dds__WirelessController_() + + smp.ly = 0.5 + for i in range(100): + pub.Write(smp, timeout=1.0) + time.sleep(0.01) + + smp.ly = 0 + pub.Write(smp) + + # smp.keys = encode_keys(["L1", "B"]) + # for i in range(3): + # pub.Write(smp, timeout=1) + # time.sleep(0.1) + + pub.Close() diff --git a/unitree_sdk2py/b2/sport/sport_api.py b/unitree_sdk2py/b2/sport/sport_api.py index 7b25e7b7..acee0189 100644 --- a/unitree_sdk2py/b2/sport/sport_api.py +++ b/unitree_sdk2py/b2/sport/sport_api.py @@ -34,6 +34,7 @@ ROBOT_SPORT_API_ID_CLASSICWALK = 1049 ROBOT_SPORT_API_ID_FASTWALK = 1050 ROBOT_SPORT_API_ID_FREEEULER = 1051 +ROBOT_SPORT_API_ID_EULER = 1007 """ " error code diff --git a/unitree_sdk2py/b2/sport/sport_client.py b/unitree_sdk2py/b2/sport/sport_client.py index 1438640b..6747c2b9 100644 --- a/unitree_sdk2py/b2/sport/sport_client.py +++ b/unitree_sdk2py/b2/sport/sport_client.py @@ -57,6 +57,7 @@ def Init(self): self._RegistApi(ROBOT_SPORT_API_ID_CLASSICWALK, 0) self._RegistApi(ROBOT_SPORT_API_ID_FASTWALK, 0) self._RegistApi(ROBOT_SPORT_API_ID_FREEEULER, 0) + self._RegistApi(ROBOT_SPORT_API_ID_EULER, 0) def Damp(self): p = {} @@ -64,6 +65,15 @@ def Damp(self): code, data = self._Call(ROBOT_SPORT_API_ID_DAMP, parameter) return code + def Euler(self, roll: float, pitch: float, yaw: float): + p = {} + p["r"] = roll + p["p"] = pitch + p["y"] = yaw + parameter = json.dumps(p) + code = self._CallNoReply(ROBOT_SPORT_API_ID_EULER, parameter) + return code + def BalanceStand(self): p = {} parameter = json.dumps(p) diff --git a/unitree_sdk2py/core/channel.py b/unitree_sdk2py/core/channel.py index 64577f99..e5043d8b 100644 --- a/unitree_sdk2py/core/channel.py +++ b/unitree_sdk2py/core/channel.py @@ -28,11 +28,13 @@ """ " class Channel """ + + class Channel: - """ " internal class __Reader """ + class __Reader: def __init__(self): self.__reader = None @@ -41,8 +43,15 @@ def __init__(self): self.__queueEnable = False self.__threadEvent = None self.__threadReader = None - - def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None, handler: Callable = None, queueLen: int = 0): + + def Init( + self, + participant: DomainParticipant, + topic: Topic, + qos: Qos = None, + handler: Callable = None, + queueLen: int = 0, + ): if handler is None: self.__reader = DataReader(participant, topic, qos) else: @@ -51,9 +60,18 @@ def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None, ha self.__queueEnable = True self.__queue = BQueue(queueLen) self.__threadEvent = Event() - self.__threadReader = Thread(target=self.__ChannelReaderThreadFunc, name="ch_reader", daemon=True) + self.__threadReader = Thread( + target=self.__ChannelReaderThreadFunc, + name="ch_reader", + daemon=True, + ) self.__threadReader.start() - self.__reader = DataReader(participant, topic, qos, Listener(on_data_available=self.__OnDataAvailable)) + self.__reader = DataReader( + participant, + topic, + qos, + Listener(on_data_available=self.__OnDataAvailable), + ) def Read(self, timeout: float = None): sample = None @@ -98,7 +116,7 @@ def __OnDataAvailable(self, reader: DataReader): if samples is None: return - # check invalid sample + # check invalid sample sample = samples[0] if isinstance(sample, InvalidSample): return @@ -118,13 +136,19 @@ def __ChannelReaderThreadFunc(self): """ " internal class __Writer """ + class __Writer: def __init__(self): self.__writer = None self.__publication_matched_count = 0 - + def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None): - self.__writer = DataWriter(participant, topic, qos, Listener(on_publication_matched=self.__OnPublicationMatched)) + self.__writer = DataWriter( + participant, + topic, + qos, + Listener(on_publication_matched=self.__OnPublicationMatched), + ) time.sleep(0.2) def Write(self, sample: Any, timeout: float = None): @@ -137,7 +161,7 @@ def Write(self, sample: Any, timeout: float = None): # print(time.time()) # check waitsec - if timeout is not None and waitsec <= 0.0: + if timeout is not None and waitsec <= -0.1: return False try: @@ -150,17 +174,20 @@ def Write(self, sample: Any, timeout: float = None): return False return True - + def Close(self): if self.__writer is not None: del self.__writer - - def __OnPublicationMatched(self, writer: DataWriter, status: dds_c_t.publication_matched_status): - self.__publication_matched_count = status.current_count + def __OnPublicationMatched( + self, writer: DataWriter, status: dds_c_t.publication_matched_status + ): + self.__publication_matched_count = status.current_count # channel __init__ - def __init__(self, participant: DomainParticipant, name: str, type: Any, qos: Qos = None): + def __init__( + self, participant: DomainParticipant, name: str, type: Any, qos: Qos = None + ): self.__reader = self.__Reader() self.__writer = self.__Writer() self.__participant = participant @@ -171,7 +198,7 @@ def SetWriter(self, qos: Qos = None): def SetReader(self, qos: Qos = None, handler: Callable = None, queueLen: int = 0): self.__reader.Init(self.__participant, self.__topic, qos, handler, queueLen) - + def Write(self, sample: Any, timeout: float = None): return self.__writer.Write(sample, timeout) @@ -188,6 +215,8 @@ def CloseWriter(self): """ " class ChannelFactory """ + + class ChannelFactory(Singleton): __domain = None __participant = None @@ -244,7 +273,9 @@ def CreateSendChannel(self, name: str, type: Any): channel.SetWriter(None) return channel - def CreateRecvChannel(self, name: str, type: Any, handler: Callable = None, queueLen: int = 0): + def CreateRecvChannel( + self, name: str, type: Any, handler: Callable = None, queueLen: int = 0 + ): channel = self.CreateChannel(name, type) channel.SetReader(None, handler, queueLen) return channel @@ -253,6 +284,8 @@ def CreateRecvChannel(self, name: str, type: Any, handler: Callable = None, queu """ " class ChannelPublisher """ + + class ChannelPublisher: def __init__(self, name: str, type: Any): factory = ChannelFactory() @@ -271,9 +304,12 @@ def Close(self): def Write(self, sample: Any, timeout: float = None): return self.__channel.Write(sample, timeout) + """ " class ChannelSubscriber """ + + class ChannelSubscriber: def __init__(self, name: str, type: Any): factory = ChannelFactory() @@ -292,9 +328,12 @@ def Close(self): def Read(self, timeout: int = None): return self.__channel.Read(timeout) + """ " function ChannelFactoryInitialize. used to intialize channel everenment. """ + + def ChannelFactoryInitialize(id: int = 0, networkInterface: str = None): factory = ChannelFactory() if not factory.Init(id, networkInterface): diff --git a/unitree_sdk2py/rpc/client_stub.py b/unitree_sdk2py/rpc/client_stub.py index 67c2c8fb..9b180fd0 100644 --- a/unitree_sdk2py/rpc/client_stub.py +++ b/unitree_sdk2py/rpc/client_stub.py @@ -14,6 +14,8 @@ """ " class ClientStub """ + + class ClientStub: def __init__(self, serviceName: str): self.__serviceName = serviceName @@ -27,12 +29,17 @@ def Init(self): self.__futureQueue = RequestFutureQueue() # create channel - self.__sendChannel = factory.CreateSendChannel(GetClientChannelName(self.__serviceName, ChannelType.SEND), Request) - self.__recvChannel = factory.CreateRecvChannel(GetClientChannelName(self.__serviceName, ChannelType.RECV), Response, - self.__ResponseHandler,10) + self.__sendChannel = factory.CreateSendChannel( + GetClientChannelName(self.__serviceName, ChannelType.SEND), Request + ) + self.__recvChannel = factory.CreateRecvChannel( + GetClientChannelName(self.__serviceName, ChannelType.RECV), + Response, + self.__ResponseHandler, + 10, + ) time.sleep(0.5) - def Send(self, request: Request, timeout: float): if self.__sendChannel.Write(request, timeout): return True diff --git a/unitree_sdk2py/test/client/vui_client_example.py b/unitree_sdk2py/test/client/vui_client_example.py index 6df7c094..6970bfbf 100644 --- a/unitree_sdk2py/test/client/vui_client_example.py +++ b/unitree_sdk2py/test/client/vui_client_example.py @@ -5,7 +5,7 @@ from unitree_sdk2py.go2.vui.vui_client import VuiClient if __name__ == "__main__": - ChannelFactoryInitialize(0, "enp2s0") + ChannelFactoryInitialize(0, "en7") client = VuiClient() client.SetTimeout(3.0) @@ -35,7 +35,7 @@ print("#################SetBrightness 0####################") - code = client.SetBrightness(0) + code = client.SetBrightness(0) if code != 0: print("set brightness error. code:", code) @@ -66,7 +66,7 @@ print("#################SetVolume 0####################") - code = client.SetVolume(0) + code = client.SetVolume(0) if code != 0: print("set volume error. code:", code) diff --git a/unitree_sdk2py/utils/crc.py b/unitree_sdk2py/utils/crc.py index c2507065..809c3115 100644 --- a/unitree_sdk2py/utils/crc.py +++ b/unitree_sdk2py/utils/crc.py @@ -12,41 +12,50 @@ import os import platform + class CRC(Singleton): def __init__(self): - #4 bytes aligned, little-endian format. - #size 812 - 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' - - + # 4 bytes aligned, little-endian format. + # size 812 + 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" + script_dir = os.path.dirname(os.path.abspath(__file__)) self.platform = platform.system() if self.platform == "Linux": - if platform.machine()=="x86_64": - self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_amd64.so') - elif platform.machine()=="aarch64": - self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_aarch64.so') - - self.crc_lib.crc32_core.argtypes = (ctypes.POINTER(ctypes.c_uint32), ctypes.c_uint32) + if platform.machine() == "x86_64": + self.crc_lib = ctypes.CDLL(script_dir + "/lib/crc_amd64.so") + elif platform.machine() == "aarch64": + self.crc_lib = ctypes.CDLL(script_dir + "/lib/crc_aarch64.so") + + self.crc_lib.crc32_core.argtypes = ( + ctypes.POINTER(ctypes.c_uint32), + ctypes.c_uint32, + ) self.crc_lib.crc32_core.restype = ctypes.c_uint32 - + def Crc(self, msg: idl.IdlStruct): - if msg.__idl_typename__ == 'unitree_go.msg.dds_.LowCmd_': + if msg.__idl_typename__ == "unitree_go.msg.dds_.LowCmd_": return self.__Crc32(self.__PackLowCmd(msg)) - elif msg.__idl_typename__ == 'unitree_go.msg.dds_.LowState_': + elif msg.__idl_typename__ == "unitree_go.msg.dds_.LowState_": return self.__Crc32(self.__PackLowState(msg)) - if msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowCmd_': + if msg.__idl_typename__ == "unitree_hg.msg.dds_.LowCmd_": return self.__Crc32(self.__PackHGLowCmd(msg)) - elif msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowState_': + elif msg.__idl_typename__ == "unitree_hg.msg.dds_.LowState_": return self.__Crc32(self.__PackHGLowState(msg)) else: - raise TypeError('unknown IDL message type to crc') + raise TypeError("unknown IDL message type to crc") def __PackLowCmd(self, cmd: LowCmd_): origData = [] @@ -86,13 +95,13 @@ def __PackLowState(self, state: LowState_): origData.extend(state.sn) origData.extend(state.version) origData.append(state.bandwidth) - + 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(20): origData.append(state.motor_state[i].mode) origData.append(state.motor_state[i].q) @@ -115,7 +124,7 @@ def __PackLowState(self, state: LowState_): origData.extend(state.bms_state.bq_ntc) origData.extend(state.bms_state.mcu_ntc) origData.extend(state.bms_state.cell_vol) - + origData.extend(state.foot_force) origData.extend(state.foot_force_est) origData.append(state.tick) @@ -157,13 +166,13 @@ def __PackHGLowState(self, state: HGLowState_): 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) @@ -184,10 +193,15 @@ def __PackHGLowState(self, state: HGLowState_): def __Trans(self, packData): calcData = [] - calcLen = ((len(packData)>>2)-1) + calcLen = (len(packData) >> 2) - 1 for i in range(calcLen): - d = ((packData[i*4+3] << 24) | (packData[i*4+2] << 16) | (packData[i*4+1] << 8) | (packData[i*4])) + d = ( + (packData[i * 4 + 3] << 24) + | (packData[i * 4 + 2] << 16) + | (packData[i * 4 + 1] << 8) + | (packData[i * 4]) + ) calcData.append(d) return calcData @@ -195,7 +209,7 @@ def __Trans(self, packData): def _crc_py(self, data): bit = 0 crc = 0xFFFFFFFF - polynomial = 0x04c11db7 + polynomial = 0x04C11DB7 for i in range(len(data)): bit = 1 << 31 @@ -212,13 +226,13 @@ def _crc_py(self, data): crc ^= polynomial bit >>= 1 - + return crc def _crc_ctypes(self, data): uint32_array = (ctypes.c_uint32 * len(data))(*data) length = len(data) - crc=self.crc_lib.crc32_core(uint32_array, length) + crc = self.crc_lib.crc32_core(uint32_array, length) return crc def __Crc32(self, data):