update zkmetaapi
This commit is contained in:
parent
f2c18a47cd
commit
d8c53dab88
|
|
@ -1,43 +1,24 @@
|
|||
import sys
|
||||
'''
|
||||
Author: liu xiaoyan
|
||||
Date: 2023-12-01 14:30:12
|
||||
LastEditors: WZX 17839623189@168.com
|
||||
LastEditTime: 2023-12-05 17:01:44
|
||||
FilePath: /wzx/zkmetaapi/wukong/ZKMetaUnit/ZKMeta.py
|
||||
'''
|
||||
import os, sys
|
||||
import time
|
||||
import math
|
||||
from typing import List
|
||||
sys.path.append("../ZKMetaUnit")
|
||||
import src.robot_interface as sdk
|
||||
from utils import MQTT, Gyro, parse_robot_state
|
||||
import math
|
||||
from .utils import MQTT, parse_robot_state, calcu_vecloty, calcu_distance
|
||||
import ZKMetaUnit.src.robot_interface as sdk
|
||||
|
||||
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.MQTT = MQTT()
|
||||
self.cmd = sdk.HighCmd()
|
||||
self.state = sdk.HighState()
|
||||
self.udp.InitCmdData(self.cmd)
|
||||
|
|
@ -52,8 +33,13 @@ class ZKDOG:
|
|||
self.cmd.yawSpeed = 0.0
|
||||
self.cmd.reserve = 0
|
||||
self.flag = 1
|
||||
self.mq = MQTT()
|
||||
self.mq.run()
|
||||
self.MQTT.run()
|
||||
|
||||
def check_temp(self):
|
||||
while True:
|
||||
time.sleep(1) # Check every 1 second
|
||||
with self.MQTT.lock:
|
||||
return parse_robot_state(self.MQTT.bytes)
|
||||
|
||||
def moving_unlocked(self, times: int = 1000):
|
||||
while times > 0:
|
||||
|
|
@ -78,191 +64,141 @@ class ZKDOG:
|
|||
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):
|
||||
def forward(self, distance: float = 1.0, gaitType: int = 0,
|
||||
velocaity: List=[1, 0], yawSpeed: int =0, footRaiseHeight: float=0.1):
|
||||
'''
|
||||
@ description: this is a function for moving forward and backward.
|
||||
Parameters:
|
||||
@distance: moving distance.
|
||||
@gaitType: sport mode. Notice: Except not modification.
|
||||
@velocaity: except max moving speed.
|
||||
@yawSpeed: Except set the value=0 when moving.
|
||||
@footRaiseHeight: Except not modification.
|
||||
EXAMPLE 1: moving forward 0.1 m
|
||||
zkdog = ZKDOG()
|
||||
zkdog.forward(distance=0.1)
|
||||
EXAMPLE 2: moving forward 10.1 m at 1m/s
|
||||
zkdog.forward(distance=10.1, velocaity=[1, 0])
|
||||
EXAMPLE 3: moving backward 5m at 1m/s
|
||||
zkdog.forward(distance=5, velocaity=[-1, 0])
|
||||
|
||||
'''
|
||||
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
|
||||
with self.MQTT.lock:
|
||||
past_dog_info = parse_robot_state(self.MQTT.bytes)
|
||||
past_point = past_dog_info["position"]
|
||||
current_point = past_point
|
||||
if distance > 2:
|
||||
while calcu_distance(current_point, past_point) < distance - 8*(math.pow(velocaity[0], 2)/12 + math.pow(velocaity[0], 2)/8):
|
||||
time.sleep(0.1)
|
||||
self.udp.Recv()
|
||||
self.udp.GetRecv(self.state)
|
||||
self.recovery()
|
||||
self.cmd.mode = 2
|
||||
self.cmd.gaitType = gaitType
|
||||
current_point = parse_robot_state(self.MQTT.bytes)["position"]
|
||||
self.cmd.velocity = velocaity
|
||||
self.cmd.yawSpeed = yawSpeed
|
||||
self.cmd.footRaiseHeight = footRaiseHeight
|
||||
self.udp.SetSend(self.cmd)
|
||||
self.udp.Send()
|
||||
current_point = parse_robot_state(self.MQTT.bytes)["position"]
|
||||
time.sleep(2)
|
||||
current_point = parse_robot_state(self.MQTT.bytes)["position"]
|
||||
else:
|
||||
while distance - calcu_distance(current_point, past_point) > 0.1:
|
||||
time.sleep(0.1)
|
||||
self.udp.Recv()
|
||||
self.udp.GetRecv(self.state)
|
||||
self.recovery()
|
||||
self.cmd.mode = 2
|
||||
self.cmd.gaitType = gaitType
|
||||
current_point = parse_robot_state(self.MQTT.bytes)["position"]
|
||||
self.cmd.velocity = [0.15 if velocaity[0]>0 else -0.15, 0]
|
||||
self.cmd.yawSpeed = yawSpeed
|
||||
self.cmd.footRaiseHeight = footRaiseHeight
|
||||
self.udp.SetSend(self.cmd)
|
||||
self.udp.Send()
|
||||
current_point = parse_robot_state(self.MQTT.bytes)["position"]
|
||||
self.recovery()
|
||||
self.udp.SetSend(self.cmd)
|
||||
self.udp.Send()
|
||||
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,
|
||||
def rolling(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 {*}
|
||||
'''
|
||||
|
||||
@ description: rolling
|
||||
Parameters:
|
||||
@tag: Rotation Angle
|
||||
@gaitType: sport mode, Notice Expect not modification
|
||||
@velocaity: moving velocaity, Notice Expect not modification.
|
||||
@yawSpeed: Arc velocity. Expect range from [-3, 3].
|
||||
@footRaiseHeight: Expect range from [0, 0.8]
|
||||
|
||||
EXAMPLE: 180 degrees left at 1 rad/s
|
||||
zkdog = ZKDOG()
|
||||
zkdog.rolling(tag=180, yawSpeed=1)
|
||||
'''
|
||||
|
||||
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()}")
|
||||
|
||||
with self.MQTT.lock:
|
||||
past_dog_info = parse_robot_state(self.MQTT.bytes)
|
||||
past_yaw = past_dog_info["rpy"][-1]
|
||||
current_yaw = past_yaw
|
||||
param_rads = 180/math.pi
|
||||
send_time = 0.1
|
||||
yaw_different = current_yaw - past_yaw
|
||||
tmp_yawSpeed = yawSpeed
|
||||
count = 0
|
||||
start_time = time.time()
|
||||
while yaw_different < tag:
|
||||
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:
|
||||
# 速度控制
|
||||
if tag-yaw_different > abs(yawSpeed)*param_rads * send_time*5:
|
||||
self.cmd.yawSpeed = yawSpeed
|
||||
else:
|
||||
tmp_yawSpeed = tmp_yawSpeed * 0.8
|
||||
self.cmd.yawSpeed = tmp_yawSpeed if abs(tmp_yawSpeed) > 0.1 else 0.1 if yawSpeed > 0 else -0.1
|
||||
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()
|
||||
time.sleep(send_time)
|
||||
# 数值获取
|
||||
current_yaw = parse_robot_state(self.MQTT.bytes)["rpy"][-1]
|
||||
|
||||
with self.mq.lock:
|
||||
robot_state = parse_robot_state(self.mq.bytes)
|
||||
gyro = Gyro(yaw_start=robot_state["rpy"][2])
|
||||
if yawSpeed < 0:
|
||||
yaw_different = (count+1)*360+past_yaw-current_yaw if past_yaw-current_yaw < 0 else past_yaw-current_yaw+count*360
|
||||
|
||||
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])
|
||||
else:
|
||||
yaw_different = (count+1)*360+current_yaw-past_yaw if current_yaw-past_yaw < 0 else current_yaw - past_yaw+count*360
|
||||
end_time = time.time()
|
||||
if end_time-start_time > 10 and past_yaw-2 <= current_yaw <= past_yaw+2:
|
||||
count += 1
|
||||
start_time = time.time()
|
||||
|
||||
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()
|
||||
|
||||
self.recovery()
|
||||
self.udp.SetSend(self.cmd)
|
||||
self.udp.Send()
|
||||
self.is_sitdown = False
|
||||
self.is_standup = True
|
||||
return True
|
||||
|
||||
def sitdown(self):
|
||||
def __sitdown(self):
|
||||
'''
|
||||
description: sitdown
|
||||
return {*}
|
||||
|
|
@ -278,14 +214,13 @@ class ZKDOG:
|
|||
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):
|
||||
def __standup(self):
|
||||
'''
|
||||
description: stand up
|
||||
return {*}
|
||||
|
|
@ -300,74 +235,70 @@ class ZKDOG:
|
|||
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
|
||||
|
||||
|
||||
def turn_left(self, tag=90):
|
||||
try:
|
||||
res = self.rolling(tag=tag)
|
||||
return {"type": res}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
def turn_right(self, tag=90):
|
||||
try:
|
||||
res = self.rolling(tag=tag, yawSpeed = -1)
|
||||
return {"type": res}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
def up(self, tag=1):
|
||||
try:
|
||||
res = self.forward(distance=tag)
|
||||
return {"type": res}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
def down(self, tag=1):
|
||||
try:
|
||||
res = self.forward(distance=tag, velocaity=[-1, 0])
|
||||
return {"type": res}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
def bark(self, tag="bark"):
|
||||
try:
|
||||
tag="bark"
|
||||
if not os.system(f"ssh unitree@192.168.123.13 -t 'aplay -D plughw:2,0 /home/unitree/tmp/{tag}'.wav"):
|
||||
return {"type": True}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
def sitdown(self, tag=1):
|
||||
try:
|
||||
res = self.__sitdown()
|
||||
return {"type": res}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
def standup(self, tag=1):
|
||||
try:
|
||||
res = self.__standup()
|
||||
return {"type": res}
|
||||
except Exception as e:
|
||||
return {"type": False, "msg": e}
|
||||
|
||||
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)
|
||||
|
||||
zd = ZKDOG()
|
||||
|
||||
print(zd.down(tag=1))
|
||||
print(zd.turn_right(tag=180))
|
||||
print(zd.up(tag=1))
|
||||
print(zd.down(tag=1))
|
||||
# zd.sitdown()
|
||||
# zd.standup()
|
||||
# zd.bark()
|
||||
|
|
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue