add SwitchToUserCtrl/SwitchToInternalCtrl to G1 LocoClient

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)
+        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)

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
-"""
+"""

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)
-
+