update g1 api and g1_loco_client_example example

Author: xiaoliangstd

example/g1/high_level/g1_loco_client_example.py

@@ -21,7 +21,10 @@ class TestOption:
     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 @@ def terminal_handle(self):
             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)

unitree_sdk2py/g1/loco/g1_loco_api.py

@@ -25,6 +25,7 @@
 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

unitree_sdk2py/g1/loco/g1_loco_client.py

@@ -9,7 +9,7 @@
 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 @@ def Init(self):
         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 @@ def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0)
         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 @@ def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False):
 
     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)