374 lines
13 KiB
Python
374 lines
13 KiB
Python
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)
|
|
|