101 lines
2.8 KiB
Python
101 lines
2.8 KiB
Python
import json
|
|
|
|
from ...rpc.client import Client
|
|
from .g1_loco_api import *
|
|
|
|
"""
|
|
" class SportClient
|
|
"""
|
|
class LocoClient(Client):
|
|
def __init__(self):
|
|
super().__init__(LOCO_SERVICE_NAME, False)
|
|
|
|
|
|
def Init(self):
|
|
# set api version
|
|
self._SetApiVerson(LOCO_API_VERSION)
|
|
|
|
# regist api
|
|
self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_ID, 0)
|
|
self._RegistApi(ROBOT_API_ID_LOCO_GET_FSM_MODE, 0)
|
|
self._RegistApi(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, 0)
|
|
self._RegistApi(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, 0)
|
|
self._RegistApi(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, 0)
|
|
self._RegistApi(ROBOT_API_ID_LOCO_GET_PHASE, 0) # deprecated
|
|
|
|
self._RegistApi(ROBOT_API_ID_LOCO_SET_FSM_ID, 0)
|
|
self._RegistApi(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, 0)
|
|
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)
|
|
|
|
# 7101
|
|
def SetFsmId(self, fsm_id: int):
|
|
p = {}
|
|
p["data"] = fsm_id
|
|
parameter = json.dumps(p)
|
|
code, data = self._Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter)
|
|
return code
|
|
|
|
# 7102
|
|
def SetBalanceMode(self, balance_mode: int):
|
|
p = {}
|
|
p["data"] = balance_mode
|
|
parameter = json.dumps(p)
|
|
code, data = self._Call(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, parameter)
|
|
return code
|
|
|
|
# 7104
|
|
def SetStandHeight(self, stand_height: float):
|
|
p = {}
|
|
p["data"] = stand_height
|
|
parameter = json.dumps(p)
|
|
code, data = self._Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter)
|
|
return code
|
|
|
|
# 7105
|
|
def SetVelocity(self, vx: float, vy: float, omega: float, duration: float = 1.0):
|
|
p = {}
|
|
velocity = [vx,vy,omega]
|
|
p["velocity"] = velocity
|
|
p["duration"] = duration
|
|
parameter = json.dumps(p)
|
|
code, data = self._Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter)
|
|
return code
|
|
|
|
def Damp(self):
|
|
self.SetFsmId(1)
|
|
|
|
def Start(self):
|
|
self.SetFsmId(200)
|
|
|
|
def Squat(self):
|
|
self.SetFsmId(2)
|
|
|
|
def Sit(self):
|
|
self.SetFsmId(3)
|
|
|
|
def StandUp(self):
|
|
self.SetFsmId(4)
|
|
|
|
def ZeroTorque(self):
|
|
self.SetFsmId(0)
|
|
|
|
def StopMove(self):
|
|
self.SetVelocity(0., 0., 0.)
|
|
|
|
def HighStand(self):
|
|
UINT32_MAX = (1 << 32) - 1
|
|
self.SetStandHeight(UINT32_MAX)
|
|
|
|
def LowStand(self):
|
|
UINT32_MIN = 0
|
|
self.SetStandHeight(UINT32_MIN)
|
|
|
|
def Move(self, vx: float, vy: float, vyaw: float, continous_move: bool = False):
|
|
duration = 864000.0 if continous_move else 1
|
|
self.SetVelocity(vx, vy, vyaw, duration)
|
|
|
|
def BalanceStand(self, balance_mode: int):
|
|
self.SetBalanceMode(balance_mode)
|
|
|