diff --git a/.gitignore b/.gitignore index 5d381cc..2009ee9 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,6 @@ __pycache__/ *$py.class # C extensions -*.so # Distribution / packaging .Python diff --git a/ZKDogApi/README.md b/ZKDogApi/README.md new file mode 100644 index 0000000..c47ab4c --- /dev/null +++ b/ZKDogApi/README.md @@ -0,0 +1,13 @@ + +# zkmetaapi +this is an api call zkmeta_api by Boss Qiu. + diff --git a/ZKDogApi/src/__init.py b/ZKDogApi/src/__init.py new file mode 100644 index 0000000..e69de29 diff --git a/ZKDogApi/src/robot_interface.cpython-36m-aarch64-linux-gnu.so b/ZKDogApi/src/robot_interface.cpython-36m-aarch64-linux-gnu.so new file mode 100644 index 0000000..6dd37df Binary files /dev/null and b/ZKDogApi/src/robot_interface.cpython-36m-aarch64-linux-gnu.so differ diff --git a/ZKDogApi/src/robot_interface.cpython-39-aarch64-linux-gnu.so b/ZKDogApi/src/robot_interface.cpython-39-aarch64-linux-gnu.so new file mode 100644 index 0000000..e8697eb Binary files /dev/null and b/ZKDogApi/src/robot_interface.cpython-39-aarch64-linux-gnu.so differ diff --git a/ZKDogApi/utils/__init__.py b/ZKDogApi/utils/__init__.py new file mode 100644 index 0000000..dbb2e22 --- /dev/null +++ b/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 + diff --git a/ZKDogApi/utils/gyro.py b/ZKDogApi/utils/gyro.py new file mode 100644 index 0000000..08e3c83 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ZKDogApi/utils/mqtt_msg.py b/ZKDogApi/utils/mqtt_msg.py new file mode 100644 index 0000000..55f6e6e --- /dev/null +++ b/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() + diff --git a/ZKDogApi/utils/mqtt_msg_decode.py b/ZKDogApi/utils/mqtt_msg_decode.py new file mode 100644 index 0000000..bd53d80 --- /dev/null +++ b/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(' 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) +