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