add ZKDogApi

.gitignore

@@ -5,7 +5,6 @@ __pycache__/
 *$py.class
 
 # C extensions
-*.so
 
 # Distribution / packaging
 .Python

ZKDogApi/README.md

@@ -0,0 +1,13 @@
+<!--
+ * @Author: Liu-xiaoyan97 lxy15058247683@aliyun.com
+ * @Date: 2023-11-21 13:40:42
+ * @LastEditors: Liu-xiaoyan97 lxy15058247683@aliyun.com
+ * @LastEditTime: 2023-11-21 13:40:43
+ * @FilePath: /alg/zkmetaapi/ZKMetaUnit/README.md
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by LLM, All Rights Reserved. 
+-->
+# zkmetaapi
+this is an api call zkmeta_api by Boss Qiu.
+

ZKDogApi/utils/__init__.py

@@ -0,0 +1,14 @@
+'''
+Author: WZX 17839623189@168.com
+Date: 2023-12-01 15:24:23
+LastEditors: WZX 17839623189@168.com
+LastEditTime: 2023-12-05 14:02:46
+FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/__init__.py
+Description: 
+
+Copyright (c) 2023 by LLM, All Rights Reserved. 
+'''
+from .mqtt_msg_decode import parse_robot_state, calcu_distance, calcu_vecloty
+from .mqtt_msg import MQTT
+from .gyro import Gyro
+

ZKDogApi/utils/gyro.py

@@ -0,0 +1,50 @@
+'''
+Author: WZX 17839623189@168.com
+Date: 2023-12-03 17:23:03
+LastEditors: WZX 17839623189@168.com
+LastEditTime: 2023-12-03 17:35:15
+FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/gyro.py
+Description: 
+
+Copyright (c) 2023 by LLM, All Rights Reserved. 
+'''
+class Gyro:  
+    def __init__(self, yaw_start=0):  
+        self.yaw_start = yaw_start  
+        self.yaw_current = yaw_start  
+        self.left_turn_angle = 0  
+        self.right_turn_angle = 0  
+  
+    def update(self, yaw):  
+        # 计算当前角度与起始角度的差值  
+        # 左转
+        if self.yaw_current > 170 and yaw < 0:
+            delta_yaw = yaw+360 - self.yaw_current
+            self.left_turn_angle += delta_yaw  
+        # 右转 
+        elif self.yaw_current < -170 and yaw >0:
+            delta_yaw = 360 - (yaw - self.yaw_current)
+            self.right_turn_angle += delta_yaw  
+        # 左转
+        elif self.yaw_current < yaw:
+            delta_yaw = yaw - self.yaw_current
+            self.left_turn_angle += delta_yaw  
+        # 右转
+        elif self.yaw_current >= yaw:
+            delta_yaw =  - (yaw - self.yaw_current)
+            self.right_turn_angle += delta_yaw  
+
+        # 更新当前角度为新的角度  
+        self.yaw_current = yaw  
+  
+    def get_left_turn_angle(self):  
+        return self.left_turn_angle
+  
+    def get_right_turn_angle(self):  
+        return self.right_turn_angle
+    
+    def get_corr_left_turn_angle(self):  
+        return self.left_turn_angle - self.right_turn_angle
+  
+    def get_corr_right_turn_angle(self):  
+        return self.right_turn_angle - self.left_turn_angle

ZKDogApi/utils/mqtt_msg.py

@@ -0,0 +1,78 @@
+'''
+Author: WZX 17839623189@168.com
+Date: 2023-12-01 14:44:07
+LastEditors: Do not edit
+LastEditTime: 2023-12-04 15:47:05
+FilePath: /lxy/zkmetaapi/ZKMetaUnit/utils/mqtt_msg.py
+Description: 
+
+Copyright (c) 2023 by LLM, All Rights Reserved. 
+'''
+import random
+import threading
+import time
+import paho.mqtt.client as mqtt_client
+import os
+
+class MQTT(object):
+    def __init__(self, client_id_header = 'UApp_', broker = '192.168.123.161', port = 1883, topic = "robot/state"):
+        self.client_id = f"{client_id_header}-{str(random.randint(0, 1000))}"
+        self.broker = broker
+        self.port = port
+        self.topic = topic
+        self.bytes = ""
+        self.lock = threading.Lock() # Thread lock used to protect self.temp
+
+    def connect_mqtt(self):
+        def on_connect(client, userdata, flags, rc):
+            if rc == 0:
+                print("Connected to MQTT Broker!")
+            else:
+                print("Failed to connect, return code", rc)
+        
+        client = mqtt_client.Client(self.client_id)
+        client.on_connect = on_connect
+        client.connect(self.broker, self.port)
+        return client
+
+    def subscribe(self, client: mqtt_client):
+        def on_message(client, userdata, msg):
+            # print(f"Received `{msg.payload}` from `{msg.topic}` topic")
+            with self.lock:
+                self.bytes = msg.payload
+    
+        client.subscribe(self.topic)
+        client.on_message = on_message
+
+    def run(self):
+        client = self.connect_mqtt()
+        self.subscribe(client)
+        client.loop_start()  # Start the MQTT client using multi-threading
+
+
+if __name__=="__main__":
+    from mqtt_msg_decode import parse_robot_state
+    import csv
+    def append_to_tsv(file_path, data):
+        with open(file_path, 'a+', newline='', encoding='utf-8') as tsvfile:
+            writer = csv.writer(tsvfile, delimiter='\t')
+            writer.writerow(data)
+    mq = MQTT()
+    mq.run()
+    len = len(os.listdir("data"))
+    # View the value of msg.payload asynchronously in another thread
+    def check_temp():
+        while True:
+            time.sleep(0.5)  # Check every 1 second
+            with mq.lock:
+                # print("mq.temp:", mq.bytes, type(mq.bytes))
+                tmp_data = parse_robot_state(mq.bytes)["velocity"]
+                tmp_data.append(time.time())
+                # append_to_tsv(f"data/result_{len}.tsv", tmp_data)
+                # print(f"time:   {tmp_data[-1]}    velocity: {tmp_data[:-1]}")
+                print(f"position {parse_robot_state(mq.bytes)['position']},   time  {time.time()} ")
+
+    check_temp()
+    # thread = threading.Thread(target=check_temp)
+    # thread.start()
+

ZKDogApi/utils/mqtt_msg_decode.py

@@ -0,0 +1,35 @@
+'''
+Author: liu xiaoyan
+Date: 2023-12-01 14:32:04
+LastEditors: WZX 17839623189@168.com
+LastEditTime: 2023-12-05 14:02:25
+FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/mqtt_msg_decode.py
+'''
+import binascii
+import struct
+import math
+
+def calcu_distance(point_a, point_b):
+    return math.sqrt(math.pow(point_a[0]-point_b[0], 2)+math.pow(point_a[1]-point_b[1], 2))
+
+def calcu_vecloty(a, b):
+    return math.sqrt(math.pow(a, 2) + math.pow(b, 2))
+
+def parse_robot_state(payload):
+    msg_payload = binascii.hexlify(payload)
+    data_bytes = bytes.fromhex(msg_payload.decode())
+    rpy = [struct.unpack_from('<h', data_bytes, offset=2*t)[0] for t in range(3)]
+    motorTotations = [struct.unpack_from('<h', data_bytes, offset=2*t+6)[0] for t in range(12)]
+    position = [round(struct.unpack_from('<f', data_bytes, offset=4*t+52)[0], 3) for t in range(3)]
+    bodyHeight = round(struct.unpack_from('<f', data_bytes, offset=64)[0], 4)
+    velocity = [round(struct.unpack_from('<f', data_bytes, offset=4*t+68)[0], 3) for t in range(3)]
+    return{
+        "rpy": rpy,
+        "forward_left": motorTotations[3:6],
+        "forward_right": motorTotations[0:3],
+        "back_left": motorTotations[9:],
+        "back_right": motorTotations[6:9],
+        "position": position,
+        "bodyHeight": bodyHeight,
+        "velocity": velocity
+    }

ZKDogApi/zkmeta.py

@@ -0,0 +1,373 @@
+import sys
+import time
+from typing import List
+sys.path.append("../ZKMetaUnit")
+import src.robot_interface as sdk
+from utils import MQTT, Gyro, parse_robot_state
+import math  
+HIGHLEVEL = 0xee
+LOWLEVEL  = 0xff
+
+def calculate_movement(gyro_data, accel_data, time_step, initial_angle=0):  
+        """  
+        Calculate the movement of an object based on gyroscope and accelerometer data.  
+        
+        Parameters:  
+        gyro_data (list): List of gyroscope data (in radians).  
+        accel_data (list): List of accelerometer data (in m/s^2).  
+        time_step (float): Time step in seconds.  
+        initial_angle (float): Initial angle in radians.  
+        
+        Returns:  
+        (movement_x, movement_y, movement_z): The movement of the object in each axis.  
+        """  
+        movement = [0, 0, 0]  # x, y, z movement  
+        angle = initial_angle  # Current angle  
+    
+        for i in range(len(gyro_data)):  
+            if i == 0:  
+                last_angle = angle  
+            else:  
+                angle += gyro_data[i] * time_step  # Update angle  
+                movement[2] += (angle - last_angle) * accel_data[i] * time_step  # Calculate z movement using angle and acceleration  
+                last_angle = angle  
+            movement[0] += math.sin(angle) * accel_data[i] * time_step  # Calculate x movement using angle and acceleration  
+            movement[1] -= math.cos(angle) * accel_data[i] * time_step  # Calculate y movement using angle and acceleration  
+        return movement
+
+class ZKDOG:
+    def __init__(self) -> None:
+        self.udp = sdk.UDP(HIGHLEVEL, 8080, "192.168.123.161", 8082)
+        self.cmd = sdk.HighCmd()
+        self.state = sdk.HighState()
+        self.udp.InitCmdData(self.cmd)
+        self.udp.Recv()
+        self.udp.GetRecv(self.state)
+        self.cmd.gaitType = 0
+        self.cmd.speedLevel = 0
+        self.cmd.footRaiseHeight = 0
+        self.cmd.bodyHeight = 0
+        self.cmd.euler = [0, 0, 0]
+        self.cmd.velocity = [0, 0]
+        self.cmd.yawSpeed = 0.0
+        self.cmd.reserve = 0
+        self.flag = 1
+        self.mq = MQTT()
+        self.mq.run()
+    
+    def moving_unlocked(self, times: int = 1000):
+        while times > 0:
+            time.sleep(0.002)
+            self.udp.Recv()
+            self.udp.GetRecv(self.state)
+            self.recovery()
+            self.cmd.mode = 6
+            times -= 1
+            self.udp.SetSend(self.cmd)
+            self.udp.Send()
+    
+    def recovery(self):
+        self.cmd.mode = 0
+        self.cmd.gaitType = 0
+        self.cmd.speedLevel = 0
+        self.cmd.footRaiseHeight = 0
+        self.cmd.bodyHeight = 0
+        self.cmd.euler = [0, 0, 0]
+        self.cmd.velocity = [0, 0]
+        self.cmd.yawSpeed = 0.0
+        self.cmd.reserve = 0
+
+
+    def forward(self, times: int = 1000, gaitType: int = 0, 
+                velocaity: List=[0.1, 0], yawSpeed: int =0, footRaiseHeight: float=0.1):
+        '''
+        description: moving 
+        @param times 
+        @gaitType DO NOT MODIFICATION
+        @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward.
+                         Notice: do not modify the second value
+        @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。
+                         0 means do not rotate.
+        @footRaiseHeight float Foot elevation. Must be positive numbers.
+        return {*}
+        '''        
+        if self.flag == 0:
+            return False
+        if self.flag == 1:
+            self.moving_unlocked()
+            while times > 0:
+                time.sleep(0.05)
+                self.udp.Recv()
+                self.udp.GetRecv(self.state)
+
+                self.recovery()
+                self.cmd.mode = 2
+                self.cmd.gaitType = gaitType
+                self.cmd.velocity = velocaity # -1  ~ +1
+                self.cmd.yawSpeed = yawSpeed
+                self.cmd.footRaiseHeight = footRaiseHeight
+                self.udp.SetSend(self.cmd)
+                self.udp.Send()
+                times -= 1
+            self.is_sitdown = False
+            self.is_standup = True
+            return True
+    
+
+    def move(self, tag: int = 50, gaitType: int = 0, 
+                velocaity: List=[0.1, 0], yawSpeed: int = 0 , footRaiseHeight: float=0.1):
+        '''
+        description: moving 
+        @param times 
+        @gaitType DO NOT MODIFICATION
+        @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward.
+                         Notice: do not modify the second value
+        @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。
+                         0 means do not rotate.
+        @footRaiseHeight float Foot elevation. Must be positive numbers.
+        return {*}
+        '''        
+        if self.flag == 0:
+            return False
+        if self.flag == 1:
+            self.moving_unlocked()
+            while tag > 0:
+                time.sleep(0.05)
+
+
+                with self.mq.lock:
+                    robot_state = parse_robot_state(self.mq.bytes)
+                    temp = robot_state["rpy"][2]
+
+                print(robot_state)
+                gyro_data, accel_data = [], []
+                gyro_data.append(self.state.imu.gyroscope[0])
+                accel_data.append(self.state.imu.accelerometer[0])
+                if tag ==1:
+                    print(calculate_movement(gyro_data=gyro_data, accel_data=accel_data, time_step=0.05*tag))
+
+                self.udp.Recv()
+                self.udp.GetRecv(self.state)
+
+                self.recovery()
+                self.cmd.mode = 2
+                self.cmd.gaitType = gaitType
+                if tag <= 20:
+                    self.cmd.velocity = 0.05
+                else:
+                    self.cmd.velocity = velocaity # -1  ~ +1
+                self.cmd.yawSpeed = yawSpeed
+                self.cmd.footRaiseHeight = footRaiseHeight
+                self.udp.SetSend(self.cmd)
+                self.udp.Send()
+                tag -= 1
+            self.is_sitdown = False
+            self.is_standup = True
+            return True
+        
+    def turn_left(self, tag: int = 90, gaitType: int = 0, 
+                velocaity: List=[0, 0], yawSpeed: int = 1, footRaiseHeight: float=0.1):
+        '''
+        description: moving 
+        @param times 
+        @gaitType DO NOT MODIFICATION
+        @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward.
+                         Notice: do not modify the second value
+        @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。
+                         0 means do not rotate.
+        @footRaiseHeight float Foot elevation. Must be positive numbers.
+        return {*}
+        '''        
+        
+        if self.flag == 0:
+            return False
+        if self.flag == 1:
+            self.moving_unlocked()
+
+            with self.mq.lock:
+                robot_state = parse_robot_state(self.mq.bytes)
+                temp = robot_state["rpy"][2]
+                gyro = Gyro(yaw_start=temp)  
+
+            while gyro.get_corr_left_turn_angle() < tag:
+                time.sleep(0.05)
+                with self.mq.lock:
+                    robot_state = parse_robot_state(self.mq.bytes)
+                    gyro.update(robot_state["rpy"][2]) 
+
+                # with self.mq.lock:
+                #     robot_state = parse_robot_state(self.mq.bytes)
+                #     print("temp=>",temp,robot_state["rpy"], "step=>",gyro.get_corr_left_turn_angle(), "tag=>",tag, "self.cmd.yawSpeed", self.cmd.yawSpeed)
+                #     print(f"左转角度: {gyro.get_corr_left_turn_angle()}，右转角度: {gyro.get_right_turn_angle()}")
+
+                self.udp.Recv()
+                self.udp.GetRecv(self.state)
+                self.recovery()
+                self.cmd.mode = 2
+                self.cmd.gaitType = gaitType
+                self.cmd.velocity = velocaity # -1  ~ +1
+                
+                if tag-gyro.get_corr_left_turn_angle() <= 20:
+                    self.cmd.yawSpeed = max((tag-gyro.get_corr_left_turn_angle())/20, 0.1)
+                else:
+                    self.cmd.yawSpeed = yawSpeed
+                self.cmd.footRaiseHeight = footRaiseHeight
+                self.udp.SetSend(self.cmd)
+                self.udp.Send()
+            return True
+        
+    def turn_right(self, tag: int = 90, gaitType: int = 0, 
+                velocaity: List=[0, 0], yawSpeed: int = -1, footRaiseHeight: float=0.1):
+        '''
+        description: moving 
+        @param times 
+        @gaitType DO NOT MODIFICATION
+        @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward.
+                         Notice: do not modify the second value
+        @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。
+                         0 means do not rotate.
+        @footRaiseHeight float Foot elevation. Must be positive numbers.
+        return {*}
+        '''        
+        
+        if self.flag == 0:
+            return False
+        if self.flag == 1:
+            self.moving_unlocked()
+
+            with self.mq.lock:
+                robot_state = parse_robot_state(self.mq.bytes)
+                gyro = Gyro(yaw_start=robot_state["rpy"][2])  
+
+            while gyro.get_corr_right_turn_angle() < tag:
+                time.sleep(0.05)
+                with self.mq.lock:
+                    robot_state = parse_robot_state(self.mq.bytes)
+                    gyro.update(robot_state["rpy"][2]) 
+
+                self.udp.Recv()
+                self.udp.GetRecv(self.state)
+                self.recovery()
+                self.cmd.mode = 2
+                self.cmd.gaitType = gaitType
+                self.cmd.velocity = velocaity # -1  ~ +1
+                
+                if tag-gyro.get_corr_right_turn_angle() <= 20:
+                    self.cmd.yawSpeed = min(-(tag-gyro.get_corr_right_turn_angle())/20, -0.1)
+                else:
+                    self.cmd.yawSpeed = yawSpeed
+                self.cmd.footRaiseHeight = footRaiseHeight
+                self.udp.SetSend(self.cmd)
+                self.udp.Send()
+                
+            return True
+
+    def sitdown(self):
+        '''
+        description: sitdown
+        return {*}
+        '''        
+        if self.flag == 0:
+            return False
+
+        if self.flag == 1:
+            times = 0
+            sitdown_cmd = [1, 6, 5, 7]
+            # 1 -> 6 -> 5 -> 7
+            while times < 4000:
+                time.sleep(0.002)
+                # self.recovery()
+                self.cmd.mode = sitdown_cmd[times//1000]
+                print(f"cmd mode is {self.cmd.mode}")
+                times += 1
+                self.udp.SetSend(self.cmd)
+                self.udp.Send()
+                self.flag = 0
+            return True
+
+    def standup(self):
+        '''
+        description: stand up
+        return {*}
+        '''        
+        times = 0
+        standup_cmd = [7, 5, 6]
+        if self.flag == 1:
+            return False
+        if self.flag == 0:
+            while times < 3000:
+                time.sleep(0.002)
+                self.cmd.mode = standup_cmd[times//1000]
+                times += 1
+                self.udp.SetSend(self.cmd)
+                print(self.cmd.mode)
+                self.udp.Send()
+            self.flag = 1
+            return True
+    
+    def getcurrenrstate(self):
+        return self.state.mode
+    
+if __name__=="__main__":
+    go1 = ZKDOG()
+
+    def get_args(k, v):
+        round = {
+            "turn-left": {
+                "times": v,
+                "velocaity": 0,
+                "yawSpeed": 1,
+                "footRaiseHeight": 0
+                },
+            "turn-right": {
+                "times": 90,
+                "velocaity": 0,
+                "yawSpeed": -0.5,
+                "footRaiseHeight": 0
+                },
+            "up": {
+                "times": int(450+750*float(v)),
+                "velocaity": 0.5,
+                "yawSpeed": 0,
+                "footRaiseHeight": 0.1
+                },
+            "down": {
+                "times": int(450+750*float(v)),
+                "velocaity": -0.5,
+                "yawSpeed": 0,
+                "footRaiseHeight": 0.1
+                }
+            }
+        return round[k]
+
+    def main(k, v):
+        # go1.sitdown()
+        # go1.standup()
+        # go1.forward(velocaity=[-0.2, 0], yawSpeed=-3)
+        new_args = get_args(k=k, v=v)
+        print(new_args)
+        times = new_args.get("times")
+        velocaity = new_args.get("velocaity")
+        yawSpeed = new_args.get("yawSpeed")
+        footRaiseHeight = new_args.get("footRaiseHeight")
+        res = go1.forward_copy(tag=times, velocaity=[velocaity, 0], yawSpeed=yawSpeed,
+                        footRaiseHeight = footRaiseHeight)
+
+        print(res)
+    # ************测试*******************
+    # go1.sitdown()
+    # go1.standup()
+    # # 前进
+    # main(k="up", v=0.5)
+    # 左转
+    # main(k="turn-left", v=180)
+    # go1.turn_right()
+    # go1.turn_left()
+    go1.move()
+    # 右转
+    # main(k="turn-right", v=90)
+    # # 左转
+    # main(k="turn-left", v=180)
+    # # 右转
+    # main(k="turn-right", v=180)
+