From fe5c661e982fd1dc856f24d6dd05b5f078e5c0ae Mon Sep 17 00:00:00 2001 From: Junda Liu <517443+i9@users.noreply.github.com> Date: Tue, 7 Jul 2026 22:52:46 -0700 Subject: [PATCH 1/2] add SwitchToUserCtrl/SwitchToInternalCtrl to G1 LocoClient --- unitree_sdk2py/g1/loco/g1_loco_api.py | 8 +++++++- unitree_sdk2py/g1/loco/g1_loco_client.py | 20 +++++++++++++++++++- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/unitree_sdk2py/g1/loco/g1_loco_api.py b/unitree_sdk2py/g1/loco/g1_loco_api.py index 2fdca7e3..affddb4c 100644 --- a/unitree_sdk2py/g1/loco/g1_loco_api.py +++ b/unitree_sdk2py/g1/loco/g1_loco_api.py @@ -26,7 +26,13 @@ ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104 ROBOT_API_ID_LOCO_SET_VELOCITY = 7105 ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106 +ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL = 7110 +ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL = 7111 + +INTERNAL_FSM_MODE_LAST = 0 +INTERNAL_FSM_MODE_PASSIVE = 1 +INTERNAL_FSM_MODE_WALKRUN = 2 """ " 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 index 288de7c4..50f850b7 100644 --- a/unitree_sdk2py/g1/loco/g1_loco_client.py +++ b/unitree_sdk2py/g1/loco/g1_loco_client.py @@ -29,6 +29,8 @@ def Init(self): 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) + self._RegistApi(ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL, 0) + self._RegistApi(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL, 0) # 7001 def GetFsmId(self): @@ -82,6 +84,22 @@ def SetTaskId(self, task_id: float): code, data = self._Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter) return code + # 7110 + def SwitchToUserCtrl(self): + p = {} + p["data"] = False + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL, parameter) + return code + + # 7111 + def SwitchToInternalCtrl(self, mode: int): + p = {} + p["data"] = mode + parameter = json.dumps(p) + code, data = self._Call(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL, parameter) + return code + def Damp(self): self.SetFsmId(1) @@ -134,4 +152,4 @@ def ShakeHand(self, stage: int = -1): 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 48764b3ef27ea99749ab9cd2a35fd96ff566afc0 Mon Sep 17 00:00:00 2001 From: Junda Liu <517443+i9@users.noreply.github.com> Date: Wed, 8 Jul 2026 10:46:50 -0700 Subject: [PATCH 2/2] refine and add tests --- .../g1/high_level/g1_loco_client_example.py | 34 +++++++++++++++++-- unitree_sdk2py/g1/loco/g1_loco_api.py | 9 +++-- unitree_sdk2py/g1/loco/g1_loco_client.py | 7 ++-- 3 files changed, 43 insertions(+), 7 deletions(-) diff --git a/example/g1/high_level/g1_loco_client_example.py b/example/g1/high_level/g1_loco_client_example.py index 55ee0596..34c7f6ee 100644 --- a/example/g1/high_level/g1_loco_client_example.py +++ b/example/g1/high_level/g1_loco_client_example.py @@ -4,6 +4,7 @@ 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 +from unitree_sdk2py.g1.loco.g1_loco_api import InternalFsmMode import math from dataclasses import dataclass @@ -26,6 +27,10 @@ class TestOption: TestOption(name="wave hand2", id=10), # wave hand and trun around TestOption(name="shake hand", id=11), TestOption(name="Lie2StandUp", id=12), + TestOption(name="switch to user ctrl briefly", id=13), + TestOption(name="switch to internal ctrl last", id=14), + TestOption(name="switch to internal ctrl passive", id=15), + TestOption(name="switch to internal ctrl walkrun", id=16), ] class UserInterface: @@ -113,5 +118,30 @@ def terminal_handle(self): sport_client.Damp() time.sleep(0.5) sport_client.Lie2StandUp() # When using the Lie2StandUp function, ensure that the robot faces up and the ground is hard, flat and rough. - - time.sleep(1) \ No newline at end of file + elif test_option.id == 13: + print("Before SwitchToUserCtrl:", sport_client.GetFsmId()) + try: + print("SwitchToUserCtrl:", sport_client.SwitchToUserCtrl()) + time.sleep(0.2) + print("After SwitchToUserCtrl:", sport_client.GetFsmId()) + finally: + print("Restore internal ctrl:", sport_client.SwitchToInternalCtrl(InternalFsmMode.LAST)) + time.sleep(0.2) + print("After restore:", sport_client.GetFsmId()) + elif test_option.id == 14: + print("Before SwitchToInternalCtrl:", sport_client.GetFsmId()) + print("SwitchToInternalCtrl:", sport_client.SwitchToInternalCtrl(InternalFsmMode.LAST)) + time.sleep(0.2) + print("After SwitchToInternalCtrl:", sport_client.GetFsmId()) + elif test_option.id == 15: + print("Before SwitchToInternalCtrl:", sport_client.GetFsmId()) + print("SwitchToInternalCtrl:", sport_client.SwitchToInternalCtrl(InternalFsmMode.PASSIVE)) + time.sleep(0.2) + print("After SwitchToInternalCtrl:", sport_client.GetFsmId()) + elif test_option.id == 16: + print("Before SwitchToInternalCtrl:", sport_client.GetFsmId()) + print("SwitchToInternalCtrl:", sport_client.SwitchToInternalCtrl(InternalFsmMode.WALKRUN)) + time.sleep(0.2) + print("After SwitchToInternalCtrl:", sport_client.GetFsmId()) + + time.sleep(1) diff --git a/unitree_sdk2py/g1/loco/g1_loco_api.py b/unitree_sdk2py/g1/loco/g1_loco_api.py index affddb4c..8b88ad84 100644 --- a/unitree_sdk2py/g1/loco/g1_loco_api.py +++ b/unitree_sdk2py/g1/loco/g1_loco_api.py @@ -1,3 +1,5 @@ +from enum import IntEnum as _IntEnum + """ " service name """ @@ -29,9 +31,10 @@ ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL = 7110 ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL = 7111 -INTERNAL_FSM_MODE_LAST = 0 -INTERNAL_FSM_MODE_PASSIVE = 1 -INTERNAL_FSM_MODE_WALKRUN = 2 +class InternalFsmMode(_IntEnum): + LAST = 0 + PASSIVE = 1 + WALKRUN = 2 """ " error code diff --git a/unitree_sdk2py/g1/loco/g1_loco_client.py b/unitree_sdk2py/g1/loco/g1_loco_client.py index 50f850b7..9975472c 100644 --- a/unitree_sdk2py/g1/loco/g1_loco_client.py +++ b/unitree_sdk2py/g1/loco/g1_loco_client.py @@ -93,9 +93,12 @@ def SwitchToUserCtrl(self): return code # 7111 - def SwitchToInternalCtrl(self, mode: int): + def SwitchToInternalCtrl(self, mode: InternalFsmMode): + if not isinstance(mode, InternalFsmMode): + raise TypeError("mode must be an InternalFsmMode") + p = {} - p["data"] = mode + p["data"] = int(mode) parameter = json.dumps(p) code, data = self._Call(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL, parameter) return code