145 lines
5.0 KiB
Python
Executable File
145 lines
5.0 KiB
Python
Executable File
import time
|
||
|
||
class BaseMsg:
|
||
def __init__(self, ctrl, msg):
|
||
self.ctrl = ctrl
|
||
self.msg = msg
|
||
|
||
def stop_force(self):
|
||
"""
|
||
强制停止，但是大部分场景不好用，容易导致 robot 崩坏。
|
||
"""
|
||
self.msg.mode = 0
|
||
self.msg.gait_id = 0
|
||
self.msg.duration = 0
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
self.ctrl.Wait_finish(0, 0)
|
||
|
||
def stop(self, wait_time=200):
|
||
self.msg.mode = 11
|
||
self.msg.gait_id = 27
|
||
self.msg.vel_des = [0, 0, 0]
|
||
self.msg.duration = wait_time
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
if wait_time:
|
||
time.sleep(wait_time / 1000)
|
||
|
||
def stop_smooth(self, wait_time=300):
|
||
"""平滑停止，采用逐渐减速的方式实现更柔和的停止过程"""
|
||
# 获取当前速度
|
||
current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') and self.msg.vel_des else [0, 0, 0]
|
||
|
||
# 如果当前速度已经为零，直接调用普通停止
|
||
if all(abs(v) < 0.01 for v in current_vel):
|
||
self.stop(wait_time)
|
||
return
|
||
|
||
# 计算减速步数和每步减速量
|
||
steps = 5 # 减速分5步完成
|
||
step_time = wait_time / steps
|
||
vel_step = [v / steps for v in current_vel]
|
||
|
||
# 逐步减速
|
||
for i in range(steps):
|
||
# 计算当前步骤的目标速度
|
||
target_vel = [current_vel[j] - vel_step[j] * (i + 1) for j in range(len(current_vel))]
|
||
|
||
# 发送减速命令
|
||
self.msg.mode = 11
|
||
self.msg.gait_id = 26
|
||
self.msg.vel_des = target_vel
|
||
self.msg.duration = step_time
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
time.sleep(step_time / 1000)
|
||
|
||
# 最后确保完全停止
|
||
self.msg.vel_des = [0, 0, 0]
|
||
self.msg.duration = step_time
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
time.sleep(step_time / 1000)
|
||
|
||
def lie_down(self, wait_time=500):
|
||
"""发送趴下指令
|
||
|
||
参数:
|
||
wait_time: 等待时间(毫秒)，默认为500ms
|
||
"""
|
||
assert wait_time != 0 or wait_time > 3000, "wait_time 必须在 0 or > 3000"
|
||
self.msg.mode = 7
|
||
self.msg.gait_id = 1
|
||
self.msg.duration = wait_time
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
if wait_time:
|
||
time.sleep(wait_time / 1000)
|
||
|
||
def stand_up(self):
|
||
self.msg.mode = 12
|
||
self.msg.gait_id = 0
|
||
self.msg.life_count += 1
|
||
print(self.msg.pos_des[2])
|
||
|
||
|
||
|
||
# 站立指令核心参数
|
||
self.msg.mode = 12 # 站立模式
|
||
self.msg.gait_id = 0 # 基础步态
|
||
self.msg.life_count += 1 # 确保指令唯一性
|
||
self.msg.pos_des = [0, 0, 0.3] # 目标高度30cm（关键！）
|
||
self.msg.step_height = [0.05, 0.05] # 抬腿高度5cm
|
||
self.msg.vel_des = [0, 0, 0] # 零速度
|
||
self.msg.rpy_des = [0, 0, 0] # 水平姿态
|
||
self.msg.value = 1 # 激活标志位（根据协议文档）
|
||
self.msg.duration = 0 # 持续执行直到完成
|
||
|
||
|
||
|
||
# print("\n=== 控制指令详情 ===")
|
||
# print(f"模式(mode): {self.msg.mode}")
|
||
# print(f"步态ID(gait_id): {self.msg.gait_id}")
|
||
# print(f"接触状态(contact): {self.msg.contact}")
|
||
# print(f"生命周期(life_count): {self.msg.life_count}")
|
||
# print(f"目标速度(vel_des): {self.msg.vel_des}")
|
||
# print(f"目标姿态(rpy_des): {self.msg.rpy_des}")
|
||
# print(f"目标位置(pos_des): {self.msg.pos_des}")
|
||
# print(f"目标加速度(acc_des): {self.msg.acc_des}")
|
||
# print(f"控制点(ctrl_point): {self.msg.ctrl_point}")
|
||
# print(f"足端位姿(foot_pose): {self.msg.foot_pose}")
|
||
# print(f"步高(step_height): {self.msg.step_height}")
|
||
# print(f"参数值(value): {self.msg.value}")
|
||
|
||
|
||
self.ctrl.Send_cmd(self.msg)
|
||
print(self.msg.pos_des[2])
|
||
self.ctrl.Wait_finish(12, 0)
|
||
print(self.msg.pos_des[2])
|
||
|
||
# def stand_up(self):
|
||
# # 1. 复位系统
|
||
# self.msg.mode = 0
|
||
# self.msg.life_count += 1
|
||
# self.ctrl.Send_cmd(self.msg)
|
||
# time.sleep(1)
|
||
|
||
# # 2. 手动展开腿部（绕过软件限制）
|
||
# for _ in range(3): # 尝试3次
|
||
# # 先降到最低高度（5cm）
|
||
# self.msg.mode = 7
|
||
# self.msg.pos_des = [0, 0, 0.05] # 关键！低于当前高度
|
||
|
||
# time.sleep(1)
|
||
|
||
# # 再尝试站立
|
||
# self.msg.mode = 12
|
||
# self.msg.pos_des = [0, 0, 0.75] # 分阶段升高
|
||
# self.msg.value = 0x01 # 激活隐藏标志位（根据协议）
|
||
# self.ctrl.Send_cmd(self.msg)
|
||
# time.sleep(2)
|
||
|
||
|
||
|
||
|