add pyi interface

.gitignore

@@ -1,2 +1,3 @@
 .vscode
 build
+test

examples/lowcmd_development.cpp

@@ -16,6 +16,7 @@ int main(int argc, char *argv[]) {
     KP = arm._ctrlComp->lowcmd->kp;
     KW = arm._ctrlComp->lowcmd->kd;
     arm._ctrlComp->lowcmd->setControlGain(KP, KW);
+    // arm._ctrlComp->lowcmd->setGripperGain(KP[KP.size()-1], KW[KW.size()-1]);
     arm.sendRecvThread->shutdown();
 
     Vec6 initQ = arm.lowstate->getQ();

examples_py/arm_python_interface.cpp

@@ -51,8 +51,9 @@ PYBIND11_MODULE(unitree_arm_interface, m){
     py::class_<LowlevelState>(m, "LowlevelState")
         .def("getQ", &LowlevelState::getQ, rvp::reference_internal)
         .def("getQd", &LowlevelState::getQd, rvp::reference_internal)
-        .def("getQdd", &LowlevelState::getQdd, rvp::reference_internal)
-        .def("getQTau", &LowlevelState::getTau, rvp::reference_internal)
+        .def("getQTau", &LowlevelState::getTau, rvp::reference_internal) // typo error in the original code
+        .def("getTau", &LowlevelState::getTau, rvp::reference_internal)
+        .def("getGripperQ", &LowlevelState::getGripperQ, rvp::reference_internal)
         ;
 
     py::class_<CtrlComponents>(m, "CtrlComponents")

examples_py/example_highcmd.py

@@ -3,10 +3,10 @@ sys.path.append("../lib")
 import unitree_arm_interface
 import time
 import numpy as np
+np.set_printoptions(precision=3, suppress=True)
 
 print("Press ctrl+\ to quit process.")
 
-np.set_printoptions(precision=3, suppress=True)
 arm =  unitree_arm_interface.ArmInterface(hasGripper=True)
 armState = unitree_arm_interface.ArmFSMState
 arm.loopOn()
@@ -16,19 +16,20 @@ arm.labelRun("forward")
 arm.startTrack(armState.JOINTCTRL)
 jnt_speed = 1.0
 for i in range(0, 1000):
-    arm.jointCtrlCmd(np.array([0,0,0,-1,0,0,-1]), jnt_speed)
+    # dp = directions * speed; include 7 joints
+    arm.jointCtrlCmd([0,0,0,-1,0,0,-1], jnt_speed)
     time.sleep(arm._ctrlComp.dt)
 
 # 2. highcmd_basic : armCtrlByFSM
 arm.labelRun("forward")
 gripper_pos = 0.0
 jnt_speed = 2.0
-arm.MoveJ(np.array([0.5,0.1,0.1,0.5,-0.2,0.5]), gripper_pos, jnt_speed)
+arm.MoveJ([0.5,0.1,0.1,0.5,-0.2,0.5], gripper_pos, jnt_speed)
 gripper_pos = -1.0
 cartesian_speed = 0.5
-arm.MoveL(np.array([0,0,0,0.45,-0.2,0.2]), gripper_pos, cartesian_speed)
+arm.MoveL([0,0,0,0.45,-0.2,0.2], gripper_pos, cartesian_speed)
 gripper_pos = 0.0
-arm.MoveC(np.array([0,0,0,0.45,0,0.4]), np.array([0,0,0,0.45,0.2,0.2]), gripper_pos, cartesian_speed)
+arm.MoveC([0,0,0,0.45,0,0.4], [0,0,0,0.45,0.2,0.2], gripper_pos, cartesian_speed)
 
 # 3. highcmd_basic : armCtrlInCartesian
 arm.labelRun("forward")
@@ -36,7 +37,7 @@ arm.startTrack(armState.CARTESIAN)
 angular_vel = 0.3
 linear_vel = 0.3
 for i in range(0, 1000):
-    arm.cartesianCtrlCmd(np.array([0,0,0,0,0,-1,-1]), angular_vel, linear_vel)
+    arm.cartesianCtrlCmd([0,0,0,0,0,-1,-1], angular_vel, linear_vel)
     time.sleep(arm._ctrlComp.dt)
 
 arm.backToStart()

examples_py/example_http_client.py

@@ -0,0 +1,63 @@
+import requests
+import numpy as np
+np.set_printoptions(precision=3)
+
+url = "http://127.0.0.1:12000/unitree/z1"
+database = {
+    "func": "",
+    "args": {},
+}
+
+def labelRun(label):
+    assert len(label) < 10
+
+    # copy data
+    data = database.copy()
+    data["func"] = "labelRun"
+    data["args"] = {
+        "label": label,
+    }
+    return requests.post(url, json=data)
+
+def labelSave(label):
+    assert len(label) < 10
+
+    # copy data
+    data = database.copy()
+    data["func"] = "labelSave"
+    data["args"] = {
+        "label": label,
+    }
+    return requests.post(url, json=data)
+
+def backToStart():
+    data = database.copy()
+    data["func"] = "backToStart"
+    return requests.post(url, json=data)
+
+def Passive():
+    data = database.copy()
+    data["func"] = "Passive"
+    return requests.post(url, json=data)
+
+def getQ():
+    data = database.copy()
+    data["func"] = "getQ"
+    return requests.post(url, json=data)
+    
+def MoveJ(q: list, gripperPos = 0, speed = 0.5):
+    assert len(q) == 6
+
+    data = database.copy()
+    data["func"] = "MoveJ"
+    data["args"] = {
+        "q": q,
+        "gripperPos": gripperPos,
+        "maxSpeed": speed,
+    }
+    return requests.post(url, json=data)
+
+# test
+labelRun("forward")
+backToStart()
+Passive()

examples_py/example_http_service.py

@@ -0,0 +1,72 @@
+import sys
+sys.path.append("../lib")
+import unitree_arm_interface
+import time
+import numpy as np
+np.set_printoptions(precision=3, suppress=True)
+
+"""
+You can use fastapi to encapsulate z1_sdk for http interface call.
+"""
+
+arm =  unitree_arm_interface.ArmInterface(hasGripper=True)
+
+from fastapi import FastAPI
+from pydantic import BaseModel
+
+app = FastAPI()
+
+class RequestDataType(BaseModel):
+    func: str # backToStart, Passive and so on
+    args: dict
+
+@app.post("/unitree/z1")
+def z1_server(item: RequestDataType):
+    try:
+        if item.func == "backToStart":
+            arm.backToStart()
+        elif item.func == "Passive":
+            arm.setFsm(unitree_arm_interface.ArmFSMState.PASSIVE)
+        elif item.func == "labelRun":
+            arm.labelRun(item.args["label"])
+        elif item.func == "labelSave":
+            arm.labelSave(item.args["label"])
+        elif item.func == "MoveJ":
+            if 'q' in item.args.keys():
+                q = item.args["q"]
+                Tdes = arm._ctrlComp.armModel.forwardKinematics(q, 6)
+                posture = unitree_arm_interface.homoToPosture(Tdes)
+            elif 'posture' in item.args.keys():
+                posture = item.args["posture"]
+            
+            gripperPos = item.args["gripperPos"] if "gripperPos" in item.args.keys() else 0.0
+            if arm.MoveJ(posture, gripperPos, item.args["maxSpeed"]):
+                return {"func": item.func, "status": "success"}
+            else:
+                return {"func": item.func, "status": "failed"}
+        elif item.func == "MoveL":
+            posture = item.args["posture"]
+            gripperPos = item.args["gripperPos"] if "gripperPos" in item.args.keys() else 0.0
+            if arm.MoveL(posture, gripperPos, item.args["maxSpeed"]):
+                return {"func": item.func, "status": "success"}
+            else:
+                return {"func": item.func, "status": "failed"}
+        elif item.func == "MoveC":
+            gripperPos = item.args["gripperPos"] if "gripperPos" in item.args.keys() else 0.0
+            middlePosture = item.args["middlePosture"]
+            endPosture = item.args["endPosture"]
+            if arm.MoveC(middlePosture, endPosture, gripperPos, item.args["maxSpeed"]):
+                return {"func": item.func, "status": "success"}
+            else:
+                return {"func": item.func, "status": "failed"}
+        elif item.func == "getQ":
+            return {"q": arm.lowstate.getQ().tolist()}
+    except Exception as e:
+        return {"func": item.func, "status": "failed", "error": str(e)}
+
+    return {"func": item.func} # some function without return state
+
+arm.loopOn()
+import uvicorn
+uvicorn.run(app, host="0.0.0.0", port=12000)
+arm.loopOff()

examples_py/unitree_arm_interface.pyi

@@ -0,0 +1,135 @@
+import numpy as np
+from enum import IntEnum
+
+def postureToHomo(posture: np.ndarray, /) -> np.ndarray: ...
+
+def homoToPosture(T: np.ndarray, /) -> np.ndarray: ...
+
+class ArmFSMState(IntEnum):
+    INVALID = 0,
+    PASSIVE = 1,
+    JOINTCTRL = 2,
+    CARTESIAN = 3,
+    MOVEJ = 4,
+    MOVEL = 5,
+    MOVEC = 6,
+    TEACH = 7,
+    TEACHREPEAT = 8,
+    TOSTATE = 9,
+    SAVESTATE = 10,
+    TRAJECTORY = 11,
+    LOWCMD = 12
+
+class LowlevelState:
+    def getQ(self) -> np.array: ...
+
+    def getQd(self) -> np.array: ...
+    
+    def getTau(self) -> np.array: ...
+
+    def getGripperQ(self) -> np.array: ...
+
+class CtrlComponents:
+    @property
+    def armModel(self) -> Z1Model: ...
+    
+    @property
+    def dt(self) -> float: ...
+
+class Z1Model:
+    def __init__(self, endPosLocal: np.array, endEffectorMass: float, endEffectorCom: np.array, endEffectorInertia: np.ndarray, /): ...
+
+    def checkInSingularity(self, q: np.array, /) -> bool: ...
+
+    def forwardKinematics(self, q: np.array, index, /) -> np.ndarray: ...
+
+    def inverseKinematics(self, Tdes: np.ndarray, qPast: np.array, checkInWrokSpace: bool, /) -> bool: ...
+
+    def solveQP(self, twist: np.array, qPast: np.array, dt: float, /): ...
+
+    def CalcJacobian(self, q: np.array, /) -> np.ndarray: ...
+
+    def inverseDynamics(self, q: np.array, qd: np.array, qdd: np.array, Ftip: np.array, /) -> np.array: ...
+
+    def jointProtect(self, q: np.array, qd: np.array, /): ...
+
+    def getJointQMax(self) -> np.array: ...
+
+    def getJointQMin(self) -> np.array: ...
+
+    def getJointSpeedMax(self) -> np.array: ...
+
+class ArmInterface:
+    def __init__(self, hasGripper: bool, /) -> None: ...
+
+    def setFsm(self, fsm: ArmFSMState): ...
+
+    def setFsmLowcmd(self): ...
+
+    def getCurrentState(self) -> ArmFSMState: ...
+
+    def loopOn(self): ...
+    
+    def loopOff(self): ...
+
+    def backToStart(self): ...
+
+    def labelRun(self, label: str, /): ...
+
+    def labelSave(self, label: str, /): ...
+
+    def teach(self, label: str, /): ...
+
+    def teachRepeat(self, label: str, /): ...
+
+    def calibration(self): ...
+
+    def MoveJ(self, posture: np.ndarray, maxSpeed: float, /) -> bool: ...
+
+    def MoveJ(self, posture: np.ndarray, gripperPos: float, maxSpeed: float, /) -> bool: ...
+
+    def MoveL(self, posture: np.ndarray, maxSpeed: float, /) -> bool: ...
+
+    def MoveL(self, posture: np.ndarray, gripperPos: float, maxSpeed: float, /) -> bool: ...
+
+    def MoveC(self, middlePosture: np.ndarray, endPosture: np.ndarray, maxSpeed: float, /) -> bool: ...
+
+    def MoveC(self, middlePosture: np.ndarray, endPosture: np.ndarray, gripperPos: float, maxSpeed: float, /) -> bool: ...
+
+    def startTrack(self, fsm: ArmFSMState, /): ...
+
+    def sendRecv(self): ...
+
+    def setWait(self, Y_N: bool, /): ...
+
+    def jointCtrlCmd(self, directions: np.ndarray, jointSpeed: float, /): ...
+
+    def cartesianCtrlCmd(self, directions: np.ndarray, oriSpeed: float, posSpeed: float, /): ...
+
+    def setArmCmd(self, q: np.ndarray, qd: np.ndarray, tau: np.ndarray, /): ...
+
+    def setGripperCmd(self, gripperPos: float, gripperW: float, gripperTau: float, /): ...
+
+    @property
+    def lowstate(self) -> LowlevelState: ...
+
+    @property
+    def _ctrlComp(self) -> CtrlComponents: ...
+
+    @property
+    def q(self) -> np.array: ...
+
+    @property
+    def qd(self) -> np.array: ...
+
+    @property
+    def tau(self) -> np.array: ...
+
+    @property
+    def gripperQ(self) -> float: ...
+
+    @property
+    def gripperQd(self) -> float: ...
+
+    @property
+    def gripperTau(self) -> float: ...