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)