From 1e06cc62e22b96d456cb29eac0b2786fc854efbb Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 23 Oct 2024 17:46:45 +0800 Subject: [PATCH 01/17] [update] update dex3-1 idl. --- unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py | 1 + unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py | 5 ++++- unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py index a8c74e6..044afcb 100644 --- a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandCmd_.py @@ -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] diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py index b4e65d6..351dc75 100644 --- a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_HandState_.py @@ -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] diff --git a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py index d260568..7dcf794 100644 --- a/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py +++ b/unitree_sdk2py/idl/unitree_hg/msg/dds_/_PressSensorState_.py @@ -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 From ac679a0c611ba2f3606dcac0aa00c52b59cbce47 Mon Sep 17 00:00:00 2001 From: Agnel Wang <59766821+Agnel-Wang@users.noreply.github.com> Date: Mon, 28 Oct 2024 15:05:12 +0800 Subject: [PATCH 02/17] v1.0.1 support for Pypi --- .gitignore | 3 +++ README.md | 6 ++++++ pyproject.toml | 3 +++ setup.py | 11 ++++++++--- 4 files changed, 20 insertions(+), 3 deletions(-) create mode 100644 pyproject.toml diff --git a/.gitignore b/.gitignore index 9871dce..001de9a 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,6 @@ __pycache__ # JetBrains IDE .idea/ + +# python +dist/ \ No newline at end of file diff --git a/README.md b/README.md index 2dda4da..31bb8bb 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,12 @@ Python interface for unitree sdk2 - numpy - opencv-python ## Install unitree_sdk2_python + +```bash +pip install unitree_sd2py +``` + +### Installing from source Execute the following commands in the terminal: ```bash cd ~ diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..07de284 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools", "wheel"] +build-backend = "setuptools.build_meta" \ No newline at end of file diff --git a/setup.py b/setup.py index 3c774e0..54d8cac 100644 --- a/setup.py +++ b/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", ], - ) + ) \ No newline at end of file From e70a48491e4b92bb830c0a6544355ca4ca366584 Mon Sep 17 00:00:00 2001 From: Agnel Wang <59766821+Agnel-Wang@users.noreply.github.com> Date: Mon, 28 Oct 2024 15:07:40 +0800 Subject: [PATCH 03/17] fix typo error --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 31bb8bb..fb799c2 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ Python interface for unitree sdk2 ## Install unitree_sdk2_python ```bash -pip install unitree_sd2py +pip install unitree_sdk2py ``` ### Installing from source From 6cf6a184713ab0fc69336f5106f9578a620a677e Mon Sep 17 00:00:00 2001 From: yangning wu Date: Tue, 5 Nov 2024 12:07:55 +0800 Subject: [PATCH 04/17] add MotorCmds and MotorState idl & add joystick tool & fix typos --- .../idl/unitree_go/msg/dds_/_MotorCmds_.py | 23 ++ .../idl/unitree_go/msg/dds_/_MotorStates_.py | 23 ++ .../idl/unitree_go/msg/dds_/__init__.py | 4 +- unitree_sdk2py/utils/crc.py | 2 +- unitree_sdk2py/utils/joystick.py | 251 ++++++++++++++++++ 5 files changed, 301 insertions(+), 2 deletions(-) create mode 100644 unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py create mode 100644 unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py create mode 100644 unitree_sdk2py/utils/joystick.py diff --git a/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py new file mode 100644 index 0000000..1dd9414 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py @@ -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_dds_wrapper.idl.unitree_go.msg.dds_.MotorCmd_'] = field(default_factory=lambda: []) + + diff --git a/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py new file mode 100644 index 0000000..51a20a4 --- /dev/null +++ b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py @@ -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_dds_wrapper.idl.unitree_go.msg.dds_.MotorState_'] = field(default_factory=lambda: []) + + diff --git a/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py b/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py index e032773..fc24f6f 100644 --- a/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py +++ b/unitree_sdk2py/idl/unitree_go/msg/dds_/__init__.py @@ -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_", ] diff --git a/unitree_sdk2py/utils/crc.py b/unitree_sdk2py/utils/crc.py index 93e4651..c250706 100644 --- a/unitree_sdk2py/utils/crc.py +++ b/unitree_sdk2py/utils/crc.py @@ -31,7 +31,7 @@ class CRC(Singleton): 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_arm64.so') + 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 diff --git a/unitree_sdk2py/utils/joystick.py b/unitree_sdk2py/utils/joystick.py new file mode 100644 index 0000000..aeda672 --- /dev/null +++ b/unitree_sdk2py/utils/joystick.py @@ -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]) + From c52f48b0b55f29dbc6ed3f67303bbd25732dc069 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Mon, 11 Nov 2024 16:49:55 +0800 Subject: [PATCH 05/17] update high-level and low-level control examples --- example/b2/high_level/b2_sport_client.py | 105 ++++++++++ example/b2/low_level/b2_stand_example.py | 175 ++++++++++++++++ .../low_level/unitree_legged_const.py | 0 example/b2w/high_level/b2w_sport_client.py | 101 +++++++++ example/b2w/low_level/b2w_stand_example.py | 196 +++++++++++++++++ example/b2w/low_level/unitree_legged_const.py | 24 +++ .../g1/high_level/g1_loco_client_example.py | 99 +++++++++ .../g1/low_level/g1_ankle_swing_example.py | 198 ++++++++++++++++++ example/{hg => g1}/readme.md | 0 example/go2/high_level/go2_sport_client.py | 109 ++++++++++ example/go2/low_level/go2_stand_example.py | 176 ++++++++++++++++ example/go2/low_level/unitree_legged_const.py | 20 ++ example/go2w/high_level/go2w_sport_client.py | 99 +++++++++ example/go2w/low_level/go2w_stand_example.py | 196 +++++++++++++++++ .../go2w/low_level/unitree_legged_const.py | 24 +++ .../h1/high_level/h1_loco_client_example.py | 96 +++++++++ example/h1/low_level/h1_low_level_example.py | 167 +++++++++++++++ example/h1/low_level/unitree_legged_const.py | 5 + example/hg/arm_sdk.py | 133 ------------ example/hg/low_cmd.py | 50 ----- example/high_level/read_highstate.py | 24 --- example/high_level/sportmode_test.py | 144 ------------- example/low_level/lowlevel_control.py | 60 ------ example/low_level/read_lowstate.py | 27 --- example/test_crc.py | 46 ---- .../comm/motion_switcher/__init__.py | 0 .../motion_switcher/motion_switcher_api.py | 29 +++ .../motion_switcher/motion_switcher_client.py | 42 ++++ unitree_sdk2py/g1/loco/g1_loco_api.py | 31 +++ unitree_sdk2py/g1/loco/g1_loco_client.py | 101 +++++++++ unitree_sdk2py/h1/loco/h1_loco_api.py | 31 +++ unitree_sdk2py/h1/loco/h1_loco_client.py | 83 ++++++++ 32 files changed, 2107 insertions(+), 484 deletions(-) create mode 100644 example/b2/high_level/b2_sport_client.py create mode 100644 example/b2/low_level/b2_stand_example.py rename example/{ => b2}/low_level/unitree_legged_const.py (100%) create mode 100644 example/b2w/high_level/b2w_sport_client.py create mode 100644 example/b2w/low_level/b2w_stand_example.py create mode 100644 example/b2w/low_level/unitree_legged_const.py create mode 100644 example/g1/high_level/g1_loco_client_example.py create mode 100644 example/g1/low_level/g1_ankle_swing_example.py rename example/{hg => g1}/readme.md (100%) create mode 100644 example/go2/high_level/go2_sport_client.py create mode 100644 example/go2/low_level/go2_stand_example.py create mode 100644 example/go2/low_level/unitree_legged_const.py create mode 100644 example/go2w/high_level/go2w_sport_client.py create mode 100644 example/go2w/low_level/go2w_stand_example.py create mode 100644 example/go2w/low_level/unitree_legged_const.py create mode 100644 example/h1/high_level/h1_loco_client_example.py create mode 100644 example/h1/low_level/h1_low_level_example.py create mode 100644 example/h1/low_level/unitree_legged_const.py delete mode 100644 example/hg/arm_sdk.py delete mode 100644 example/hg/low_cmd.py delete mode 100644 example/high_level/read_highstate.py delete mode 100644 example/high_level/sportmode_test.py delete mode 100644 example/low_level/lowlevel_control.py delete mode 100644 example/low_level/read_lowstate.py delete mode 100644 example/test_crc.py create mode 100644 unitree_sdk2py/comm/motion_switcher/__init__.py create mode 100644 unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py create mode 100644 unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py create mode 100644 unitree_sdk2py/g1/loco/g1_loco_api.py create mode 100644 unitree_sdk2py/g1/loco/g1_loco_client.py create mode 100644 unitree_sdk2py/h1/loco/h1_loco_api.py create mode 100644 unitree_sdk2py/h1/loco/h1_loco_client.py diff --git a/example/b2/high_level/b2_sport_client.py b/example/b2/high_level/b2_sport_client.py new file mode 100644 index 0000000..695d28e --- /dev/null +++ b/example/b2/high_level/b2_sport_client.py @@ -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) diff --git a/example/b2/low_level/b2_stand_example.py b/example/b2/low_level/b2_stand_example.py new file mode 100644 index 0000000..a091630 --- /dev/null +++ b/example/b2/low_level/b2_stand_example.py @@ -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) \ No newline at end of file diff --git a/example/low_level/unitree_legged_const.py b/example/b2/low_level/unitree_legged_const.py similarity index 100% rename from example/low_level/unitree_legged_const.py rename to example/b2/low_level/unitree_legged_const.py diff --git a/example/b2w/high_level/b2w_sport_client.py b/example/b2w/high_level/b2w_sport_client.py new file mode 100644 index 0000000..e324f75 --- /dev/null +++ b/example/b2w/high_level/b2w_sport_client.py @@ -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) diff --git a/example/b2w/low_level/b2w_stand_example.py b/example/b2w/low_level/b2w_stand_example.py new file mode 100644 index 0000000..b535059 --- /dev/null +++ b/example/b2w/low_level/b2w_stand_example.py @@ -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) \ No newline at end of file diff --git a/example/b2w/low_level/unitree_legged_const.py b/example/b2w/low_level/unitree_legged_const.py new file mode 100644 index 0000000..7943e31 --- /dev/null +++ b/example/b2w/low_level/unitree_legged_const.py @@ -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 diff --git a/example/g1/high_level/g1_loco_client_example.py b/example/g1/high_level/g1_loco_client_example.py new file mode 100644 index 0000000..78e3ad2 --- /dev/null +++ b/example/g1/high_level/g1_loco_client_example.py @@ -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.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) +] + +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() + + time.sleep(1) \ No newline at end of file diff --git a/example/g1/low_level/g1_ankle_swing_example.py b/example/g1/low_level/g1_ankle_swing_example.py new file mode 100644 index 0000000..9189912 --- /dev/null +++ b/example/g1/low_level/g1_ankle_swing_example.py @@ -0,0 +1,198 @@ +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 + + 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) \ No newline at end of file diff --git a/example/hg/readme.md b/example/g1/readme.md similarity index 100% rename from example/hg/readme.md rename to example/g1/readme.md diff --git a/example/go2/high_level/go2_sport_client.py b/example/go2/high_level/go2_sport_client.py new file mode 100644 index 0000000..d2d9ab0 --- /dev/null +++ b/example/go2/high_level/go2_sport_client.py @@ -0,0 +1,109 @@ +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) +] + +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() + + time.sleep(1) diff --git a/example/go2/low_level/go2_stand_example.py b/example/go2/low_level/go2_stand_example.py new file mode 100644 index 0000000..f2241ce --- /dev/null +++ b/example/go2/low_level/go2_stand_example.py @@ -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) \ No newline at end of file diff --git a/example/go2/low_level/unitree_legged_const.py b/example/go2/low_level/unitree_legged_const.py new file mode 100644 index 0000000..153a051 --- /dev/null +++ b/example/go2/low_level/unitree_legged_const.py @@ -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 diff --git a/example/go2w/high_level/go2w_sport_client.py b/example/go2w/high_level/go2w_sport_client.py new file mode 100644 index 0000000..824a882 --- /dev/null +++ b/example/go2w/high_level/go2w_sport_client.py @@ -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) diff --git a/example/go2w/low_level/go2w_stand_example.py b/example/go2w/low_level/go2w_stand_example.py new file mode 100644 index 0000000..e3a54de --- /dev/null +++ b/example/go2w/low_level/go2w_stand_example.py @@ -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) \ No newline at end of file diff --git a/example/go2w/low_level/unitree_legged_const.py b/example/go2w/low_level/unitree_legged_const.py new file mode 100644 index 0000000..7943e31 --- /dev/null +++ b/example/go2w/low_level/unitree_legged_const.py @@ -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 diff --git a/example/h1/high_level/h1_loco_client_example.py b/example/h1/high_level/h1_loco_client_example.py new file mode 100644 index 0000000..4432d8a --- /dev/null +++ b/example/h1/high_level/h1_loco_client_example.py @@ -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) \ No newline at end of file diff --git a/example/h1/low_level/h1_low_level_example.py b/example/h1/low_level/h1_low_level_example.py new file mode 100644 index 0000000..51a9111 --- /dev/null +++ b/example/h1/low_level/h1_low_level_example.py @@ -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) \ No newline at end of file diff --git a/example/h1/low_level/unitree_legged_const.py b/example/h1/low_level/unitree_legged_const.py new file mode 100644 index 0000000..c9112ea --- /dev/null +++ b/example/h1/low_level/unitree_legged_const.py @@ -0,0 +1,5 @@ +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 diff --git a/example/hg/arm_sdk.py b/example/hg/arm_sdk.py deleted file mode 100644 index ff6d6b4..0000000 --- a/example/hg/arm_sdk.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/example/hg/low_cmd.py b/example/hg/low_cmd.py deleted file mode 100644 index 331bfe5..0000000 --- a/example/hg/low_cmd.py +++ /dev/null @@ -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() diff --git a/example/high_level/read_highstate.py b/example/high_level/read_highstate.py deleted file mode 100644 index a3cec53..0000000 --- a/example/high_level/read_highstate.py +++ /dev/null @@ -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) diff --git a/example/high_level/sportmode_test.py b/example/high_level/sportmode_test.py deleted file mode 100644 index 845ddc5..0000000 --- a/example/high_level/sportmode_test.py +++ /dev/null @@ -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) diff --git a/example/low_level/lowlevel_control.py b/example/low_level/lowlevel_control.py deleted file mode 100644 index 638c5f2..0000000 --- a/example/low_level/lowlevel_control.py +++ /dev/null @@ -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) \ No newline at end of file diff --git a/example/low_level/read_lowstate.py b/example/low_level/read_lowstate.py deleted file mode 100644 index 8653822..0000000 --- a/example/low_level/read_lowstate.py +++ /dev/null @@ -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) diff --git a/example/test_crc.py b/example/test_crc.py deleted file mode 100644 index e4cf992..0000000 --- a/example/test_crc.py +++ /dev/null @@ -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) diff --git a/unitree_sdk2py/comm/motion_switcher/__init__.py b/unitree_sdk2py/comm/motion_switcher/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py b/unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py new file mode 100644 index 0000000..ddb9a42 --- /dev/null +++ b/unitree_sdk2py/comm/motion_switcher/motion_switcher_api.py @@ -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 diff --git a/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py b/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py new file mode 100644 index 0000000..74f223f --- /dev/null +++ b/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py @@ -0,0 +1,42 @@ +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 + + # 1003 + def ReleaseMode(self): + p = {} + parameter = json.dumps(p) + code, data = self._Call(MOTION_SWITCHER_API_ID_RELEASE_MODE, parameter) + + return code, None + \ No newline at end of file diff --git a/unitree_sdk2py/g1/loco/g1_loco_api.py b/unitree_sdk2py/g1/loco/g1_loco_api.py new file mode 100644 index 0000000..6b2151f --- /dev/null +++ b/unitree_sdk2py/g1/loco/g1_loco_api.py @@ -0,0 +1,31 @@ +""" +" 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 + +""" +" error code +""" \ No newline at end of file diff --git a/unitree_sdk2py/g1/loco/g1_loco_client.py b/unitree_sdk2py/g1/loco/g1_loco_client.py new file mode 100644 index 0000000..4d49ead --- /dev/null +++ b/unitree_sdk2py/g1/loco/g1_loco_client.py @@ -0,0 +1,101 @@ +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) + + + 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) + + # 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 + + 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) + \ No newline at end of file diff --git a/unitree_sdk2py/h1/loco/h1_loco_api.py b/unitree_sdk2py/h1/loco/h1_loco_api.py new file mode 100644 index 0000000..bd8ddbe --- /dev/null +++ b/unitree_sdk2py/h1/loco/h1_loco_api.py @@ -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 +""" \ No newline at end of file diff --git a/unitree_sdk2py/h1/loco/h1_loco_client.py b/unitree_sdk2py/h1/loco/h1_loco_client.py new file mode 100644 index 0000000..3bc01e3 --- /dev/null +++ b/unitree_sdk2py/h1/loco/h1_loco_client.py @@ -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) \ No newline at end of file From fae61cfa743f8255931a56f7a9e8ae6e542c9977 Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 13 Nov 2024 14:23:33 +0800 Subject: [PATCH 06/17] [fix] fix dex3-1 default value. --- unitree_sdk2py/idl/default.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/unitree_sdk2py/idl/default.py b/unitree_sdk2py/idl/default.py index f21210f..89d567d 100644 --- a/unitree_sdk2py/idl/default.py +++ b/unitree_sdk2py/idl/default.py @@ -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]) """ From fcbc0ec2f5b3a2e325a4fadd5ce77ded05c8e4fc Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Thu, 5 Dec 2024 18:17:26 +0800 Subject: [PATCH 07/17] update go2_utlidar_switch example --- example/go2/high_level/go2_utlidar_switch.py | 39 ++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 example/go2/high_level/go2_utlidar_switch.py diff --git a/example/go2/high_level/go2_utlidar_switch.py b/example/go2/high_level/go2_utlidar_switch.py new file mode 100644 index 0000000..09e0ce9 --- /dev/null +++ b/example/go2/high_level/go2_utlidar_switch.py @@ -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") + + + From 40605b4dac3193a92346be8356d2180fe0715b59 Mon Sep 17 00:00:00 2001 From: ruziniuuuuu Date: Mon, 23 Dec 2024 18:26:03 +0800 Subject: [PATCH 08/17] [fix] remove dependency on `unitree_dds_wrapper` --- unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py | 2 +- unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py index 1dd9414..11d3029 100644 --- a/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py +++ b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorCmds_.py @@ -18,6 +18,6 @@ import cyclonedds.idl.types as types @annotate.final @annotate.autoid("sequential") class MotorCmds_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorCmds_"): - cmds: types.sequence['unitree_dds_wrapper.idl.unitree_go.msg.dds_.MotorCmd_'] = field(default_factory=lambda: []) + cmds: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorCmd_'] = field(default_factory=lambda: []) diff --git a/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py index 51a20a4..4b0245b 100644 --- a/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py +++ b/unitree_sdk2py/idl/unitree_go/msg/dds_/_MotorStates_.py @@ -18,6 +18,6 @@ import cyclonedds.idl.types as types @annotate.final @annotate.autoid("sequential") class MotorStates_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorStates_"): - states: types.sequence['unitree_dds_wrapper.idl.unitree_go.msg.dds_.MotorState_'] = field(default_factory=lambda: []) + states: types.sequence['unitree_sdk2py.idl.unitree_go.msg.dds_.MotorState_'] = field(default_factory=lambda: []) From 86755a444046ef9b6d610c0b51d450aa7eadbec4 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Sat, 28 Dec 2024 18:19:06 +0800 Subject: [PATCH 09/17] update wireless_controller example --- .../wireless_controller.py | 158 +++++++++++++----- 1 file changed, 117 insertions(+), 41 deletions(-) diff --git a/example/wireless_controller/wireless_controller.py b/example/wireless_controller/wireless_controller.py index 0596d1a..995487f 100644 --- a/example/wireless_controller/wireless_controller.py +++ b/example/wireless_controller/wireless_controller.py @@ -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('> 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) \ No newline at end of file From 21e44b7c4c1a11b2b2e426ef2930fd13ce2c6a36 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Tue, 31 Dec 2024 10:45:16 +0800 Subject: [PATCH 10/17] update h1_2 and g1 low_level example --- ...ing_example.py => g1_low_level_example.py} | 15 +- .../h1_2/low_level/h1_2_low_level_example.py | 201 ++++++++++++++++++ 2 files changed, 212 insertions(+), 4 deletions(-) rename example/g1/low_level/{g1_ankle_swing_example.py => g1_low_level_example.py} (91%) create mode 100644 example/h1_2/low_level/h1_2_low_level_example.py diff --git a/example/g1/low_level/g1_ankle_swing_example.py b/example/g1/low_level/g1_low_level_example.py similarity index 91% rename from example/g1/low_level/g1_ankle_swing_example.py rename to example/g1/low_level/g1_low_level_example.py index 9189912..382e863 100644 --- a/example/g1/low_level/g1_ankle_swing_example.py +++ b/example/g1/low_level/g1_low_level_example.py @@ -102,12 +102,12 @@ class Custom: self.lowcmd_publisher_.Init() # create subscriber # - self.__lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) - self.__lowstate_subscriber.Init(self.LowStateHandler, 10) + 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" + interval=self.control_dt_, target=self.LowCmdWrite, name="control" ) while self.update_mode_machine_ == False: time.sleep(1) @@ -127,7 +127,7 @@ class Custom: self.counter_ = 0 print(self.low_state.imu_state.rpy) - def __LowCmdWrite(self): + def LowCmdWrite(self): self.time_ += self.control_dt_ if self.time_ < self.duration_ : @@ -176,6 +176,13 @@ class Custom: 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) diff --git a/example/h1_2/low_level/h1_2_low_level_example.py b/example/h1_2/low_level/h1_2_low_level_example.py new file mode 100644 index 0000000..0959b32 --- /dev/null +++ b/example/h1_2/low_level/h1_2_low_level_example.py @@ -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) \ No newline at end of file From 0d86c54625c2f88ba6424bbb567a9b4ceb2ad220 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Thu, 2 Jan 2025 21:37:07 +0800 Subject: [PATCH 11/17] upload motion_switcher_example and update MotionSwitcherClient class --- .../motionSwitcher/motion_switcher_example.py | 36 +++++++++++++++++++ .../motion_switcher/motion_switcher_client.py | 9 +++++ 2 files changed, 45 insertions(+) create mode 100644 example/motionSwitcher/motion_switcher_example.py diff --git a/example/motionSwitcher/motion_switcher_example.py b/example/motionSwitcher/motion_switcher_example.py new file mode 100644 index 0000000..4f39f72 --- /dev/null +++ b/example/motionSwitcher/motion_switcher_example.py @@ -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) + diff --git a/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py b/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py index 74f223f..1ad411a 100644 --- a/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py +++ b/unitree_sdk2py/comm/motion_switcher/motion_switcher_client.py @@ -32,6 +32,15 @@ class MotionSwitcherClient(Client): 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 = {} From 3d497a606de559bc8e29fbb689e620204d050cf3 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Mon, 13 Jan 2025 14:49:59 +0800 Subject: [PATCH 12/17] add new APIs for go2 sport client and update go2_sport_client example --- example/go2/high_level/go2_sport_client.py | 63 ++++++++++++++++- unitree_sdk2py/go2/sport/sport_api.py | 13 ++++ unitree_sdk2py/go2/sport/sport_client.py | 82 ++++++++++++++++++++++ 3 files changed, 157 insertions(+), 1 deletion(-) diff --git a/example/go2/high_level/go2_sport_client.py b/example/go2/high_level/go2_sport_client.py index d2d9ab0..b14d79d 100644 --- a/example/go2/high_level/go2_sport_client.py +++ b/example/go2/high_level/go2_sport_client.py @@ -27,7 +27,17 @@ option_list = [ 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="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: @@ -105,5 +115,56 @@ if __name__ == "__main__": 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) diff --git a/unitree_sdk2py/go2/sport/sport_api.py b/unitree_sdk2py/go2/sport/sport_api.py index c399031..bfca62a 100644 --- a/unitree_sdk2py/go2/sport/sport_api.py +++ b/unitree_sdk2py/go2/sport/sport_api.py @@ -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 diff --git a/unitree_sdk2py/go2/sport/sport_client.py b/unitree_sdk2py/go2/sport/sport_client.py index 8fea8b8..d058e51 100644 --- a/unitree_sdk2py/go2/sport/sport_client.py +++ b/unitree_sdk2py/go2/sport/sport_client.py @@ -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 \ No newline at end of file From 0f170964afe8b75fe1801b3b3e2c252974569589 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Tue, 14 Jan 2025 09:32:05 +0800 Subject: [PATCH 13/17] update g1 api and g1_loco_client_example example --- .../g1/high_level/g1_loco_client_example.py | 13 +++++++++- unitree_sdk2py/g1/loco/g1_loco_api.py | 1 + unitree_sdk2py/g1/loco/g1_loco_client.py | 25 ++++++++++++++++++- 3 files changed, 37 insertions(+), 2 deletions(-) diff --git a/example/g1/high_level/g1_loco_client_example.py b/example/g1/high_level/g1_loco_client_example.py index 78e3ad2..4b7a42d 100644 --- a/example/g1/high_level/g1_loco_client_example.py +++ b/example/g1/high_level/g1_loco_client_example.py @@ -21,7 +21,10 @@ option_list = [ 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="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: @@ -95,5 +98,13 @@ if __name__ == "__main__": 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) \ No newline at end of file diff --git a/unitree_sdk2py/g1/loco/g1_loco_api.py b/unitree_sdk2py/g1/loco/g1_loco_api.py index 6b2151f..937de19 100644 --- a/unitree_sdk2py/g1/loco/g1_loco_api.py +++ b/unitree_sdk2py/g1/loco/g1_loco_api.py @@ -25,6 +25,7 @@ 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 diff --git a/unitree_sdk2py/g1/loco/g1_loco_client.py b/unitree_sdk2py/g1/loco/g1_loco_client.py index 4d49ead..af04b05 100644 --- a/unitree_sdk2py/g1/loco/g1_loco_client.py +++ b/unitree_sdk2py/g1/loco/g1_loco_client.py @@ -9,7 +9,7 @@ from .g1_loco_api import * class LocoClient(Client): def __init__(self): super().__init__(LOCO_SERVICE_NAME, False) - + self.first_shake_hand_stage_ = -1 def Init(self): # set api version @@ -28,6 +28,7 @@ class LocoClient(Client): 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): @@ -62,6 +63,14 @@ class LocoClient(Client): 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) @@ -98,4 +107,18 @@ class LocoClient(Client): 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) \ No newline at end of file From f17114c8f592c2cde8517b25e94648e36143b600 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Tue, 14 Jan 2025 12:22:41 +0800 Subject: [PATCH 14/17] update g1 audio api and g1_audio_client_example --- example/g1/audio/g1_audio_client_example.py | 44 +++++++++++++++ unitree_sdk2py/g1/audio/g1_audio_api.py | 24 ++++++++ unitree_sdk2py/g1/audio/g1_audio_client.py | 62 +++++++++++++++++++++ 3 files changed, 130 insertions(+) create mode 100644 example/g1/audio/g1_audio_client_example.py create mode 100644 unitree_sdk2py/g1/audio/g1_audio_api.py create mode 100644 unitree_sdk2py/g1/audio/g1_audio_client.py diff --git a/example/g1/audio/g1_audio_client_example.py b/example/g1/audio/g1_audio_client_example.py new file mode 100644 index 0000000..6e0cc25 --- /dev/null +++ b/example/g1/audio/g1_audio_client_example.py @@ -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) + diff --git a/unitree_sdk2py/g1/audio/g1_audio_api.py b/unitree_sdk2py/g1/audio/g1_audio_api.py new file mode 100644 index 0000000..9aba780 --- /dev/null +++ b/unitree_sdk2py/g1/audio/g1_audio_api.py @@ -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 +""" \ No newline at end of file diff --git a/unitree_sdk2py/g1/audio/g1_audio_client.py b/unitree_sdk2py/g1/audio/g1_audio_client.py new file mode 100644 index 0000000..d8d0bc3 --- /dev/null +++ b/unitree_sdk2py/g1/audio/g1_audio_client.py @@ -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 From 3af1610228977f449e22c8fba868bb8516d41ec3 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Wed, 15 Jan 2025 09:11:51 +0800 Subject: [PATCH 15/17] update g1 arm_sdk examples --- .../g1/high_level/g1_arm5_sdk_dds_example.py | 192 +++++++++++++++++ .../g1/high_level/g1_arm7_sdk_dds_example.py | 194 ++++++++++++++++++ 2 files changed, 386 insertions(+) create mode 100644 example/g1/high_level/g1_arm5_sdk_dds_example.py create mode 100644 example/g1/high_level/g1_arm7_sdk_dds_example.py diff --git a/example/g1/high_level/g1_arm5_sdk_dds_example.py b/example/g1/high_level/g1_arm5_sdk_dds_example.py new file mode 100644 index 0000000..6090807 --- /dev/null +++ b/example/g1/high_level/g1_arm5_sdk_dds_example.py @@ -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) \ No newline at end of file diff --git a/example/g1/high_level/g1_arm7_sdk_dds_example.py b/example/g1/high_level/g1_arm7_sdk_dds_example.py new file mode 100644 index 0000000..6ced14f --- /dev/null +++ b/example/g1/high_level/g1_arm7_sdk_dds_example.py @@ -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) \ No newline at end of file From 04a4b035f1cc868975eae921bcf2e4ae58d3752a Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Wed, 19 Feb 2025 21:39:18 +0800 Subject: [PATCH 16/17] Update the examples for the Go2 front camera --- example/{ => go2}/front_camera/camera_opencv.py | 0 example/{ => go2}/front_camera/capture_image.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename example/{ => go2}/front_camera/camera_opencv.py (100%) rename example/{ => go2}/front_camera/capture_image.py (100%) diff --git a/example/front_camera/camera_opencv.py b/example/go2/front_camera/camera_opencv.py similarity index 100% rename from example/front_camera/camera_opencv.py rename to example/go2/front_camera/camera_opencv.py diff --git a/example/front_camera/capture_image.py b/example/go2/front_camera/capture_image.py similarity index 100% rename from example/front_camera/capture_image.py rename to example/go2/front_camera/capture_image.py From 4eadcf6ba0cc822501ff2f1766be637c0fb7f229 Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Thu, 20 Feb 2025 20:35:47 +0800 Subject: [PATCH 17/17] Update the examples for the B2W and B2 cameras. --- example/b2/camera/camera_opencv.py | 51 +++++++++++++++++++ example/b2/camera/capture_image.py | 51 +++++++++++++++++++ example/b2w/camera/camera_opencv.py | 51 +++++++++++++++++++ example/b2w/camera/capture_image.py | 51 +++++++++++++++++++ .../b2/back_video/back_video_api.py | 16 ++++++ .../b2/back_video/back_video_client.py | 23 +++++++++ .../b2/front_video/front_video_api.py | 16 ++++++ .../b2/front_video/front_video_client.py | 23 +++++++++ unitree_sdk2py/b2/video/video_api.py | 16 ------ unitree_sdk2py/b2/video/video_client.py | 23 --------- 10 files changed, 282 insertions(+), 39 deletions(-) create mode 100644 example/b2/camera/camera_opencv.py create mode 100644 example/b2/camera/capture_image.py create mode 100644 example/b2w/camera/camera_opencv.py create mode 100644 example/b2w/camera/capture_image.py create mode 100644 unitree_sdk2py/b2/back_video/back_video_api.py create mode 100644 unitree_sdk2py/b2/back_video/back_video_client.py create mode 100644 unitree_sdk2py/b2/front_video/front_video_api.py create mode 100644 unitree_sdk2py/b2/front_video/front_video_client.py delete mode 100644 unitree_sdk2py/b2/video/video_api.py delete mode 100644 unitree_sdk2py/b2/video/video_client.py diff --git a/example/b2/camera/camera_opencv.py b/example/b2/camera/camera_opencv.py new file mode 100644 index 0000000..3fafcef --- /dev/null +++ b/example/b2/camera/camera_opencv.py @@ -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() + diff --git a/example/b2/camera/capture_image.py b/example/b2/camera/capture_image.py new file mode 100644 index 0000000..dcfaad6 --- /dev/null +++ b/example/b2/camera/capture_image.py @@ -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) diff --git a/example/b2w/camera/camera_opencv.py b/example/b2w/camera/camera_opencv.py new file mode 100644 index 0000000..3fafcef --- /dev/null +++ b/example/b2w/camera/camera_opencv.py @@ -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() + diff --git a/example/b2w/camera/capture_image.py b/example/b2w/camera/capture_image.py new file mode 100644 index 0000000..dcfaad6 --- /dev/null +++ b/example/b2w/camera/capture_image.py @@ -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) diff --git a/unitree_sdk2py/b2/back_video/back_video_api.py b/unitree_sdk2py/b2/back_video/back_video_api.py new file mode 100644 index 0000000..16f566b --- /dev/null +++ b/unitree_sdk2py/b2/back_video/back_video_api.py @@ -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 diff --git a/unitree_sdk2py/b2/back_video/back_video_client.py b/unitree_sdk2py/b2/back_video/back_video_client.py new file mode 100644 index 0000000..e19c97f --- /dev/null +++ b/unitree_sdk2py/b2/back_video/back_video_client.py @@ -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, []) diff --git a/unitree_sdk2py/b2/front_video/front_video_api.py b/unitree_sdk2py/b2/front_video/front_video_api.py new file mode 100644 index 0000000..6f4717a --- /dev/null +++ b/unitree_sdk2py/b2/front_video/front_video_api.py @@ -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 diff --git a/unitree_sdk2py/b2/front_video/front_video_client.py b/unitree_sdk2py/b2/front_video/front_video_client.py new file mode 100644 index 0000000..5ba065a --- /dev/null +++ b/unitree_sdk2py/b2/front_video/front_video_client.py @@ -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, []) diff --git a/unitree_sdk2py/b2/video/video_api.py b/unitree_sdk2py/b2/video/video_api.py deleted file mode 100644 index a4cb1b6..0000000 --- a/unitree_sdk2py/b2/video/video_api.py +++ /dev/null @@ -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 diff --git a/unitree_sdk2py/b2/video/video_client.py b/unitree_sdk2py/b2/video/video_client.py deleted file mode 100644 index 79e1fb5..0000000 --- a/unitree_sdk2py/b2/video/video_client.py +++ /dev/null @@ -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, [])