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 2fdca7e3..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 """ @@ -26,7 +28,14 @@ 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 + +class InternalFsmMode(_IntEnum): + LAST = 0 + PASSIVE = 1 + 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..9975472c 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,25 @@ 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: InternalFsmMode): + if not isinstance(mode, InternalFsmMode): + raise TypeError("mode must be an InternalFsmMode") + + p = {} + p["data"] = int(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 +155,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 +