83 lines
2.4 KiB
Python
83 lines
2.4 KiB
Python
|
import json
|
||
|
|
|
||
|
|
from ...rpc.client import Client
|
||
|
|
from .h1_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)
|
||
|
|
|
||
|
|
# 8101
|
||
|
|
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
|
||
|
|
|
||
|
|
# 8104
|
||
|
|
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
|
||
|
|
|
||
|
|
# 8105
|
||
|
|
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(204)
|
||
|
|
|
||
|
|
def StandUp(self):
|
||
|
|
self.SetFsmId(2)
|
||
|
|
|
||
|
|
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)
|