From 19a21ef06728fa1a2ee28079f6517ac9c34de99c Mon Sep 17 00:00:00 2001
From: ZKX34 <3442977470@qq.com>
Date: Mon, 22 Dec 2025 14:35:01 +0800
Subject: [PATCH 1/7] =?UTF-8?q?=E8=AE=A9=E6=97=A0=E4=BA=BA=E6=9C=BA?=
=?UTF-8?q?=E9=A3=9E=E8=B5=B7=E6=9D=A5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/drone_simulation/MUJOCO_LOG.TXT | 3 +
src/drone_simulation/main.py | 474 ++++++++++++++++++++++++++++
src/drone_simulation/model.xml | 164 ++++++++++
3 files changed, 641 insertions(+)
create mode 100644 src/drone_simulation/MUJOCO_LOG.TXT
create mode 100644 src/drone_simulation/main.py
create mode 100644 src/drone_simulation/model.xml
diff --git a/src/drone_simulation/MUJOCO_LOG.TXT b/src/drone_simulation/MUJOCO_LOG.TXT
new file mode 100644
index 0000000000..8d7def32c0
--- /dev/null
+++ b/src/drone_simulation/MUJOCO_LOG.TXT
@@ -0,0 +1,3 @@
+Mon Dec 22 14:33:32 2025
+WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.0450.
+
diff --git a/src/drone_simulation/main.py b/src/drone_simulation/main.py
new file mode 100644
index 0000000000..e13322b3e8
--- /dev/null
+++ b/src/drone_simulation/main.py
@@ -0,0 +1,474 @@
+"""
+MuJoCo 四旋翼无人机仿真 - 修复纹理错误版本
+"""
+
+import mujoco
+import mujoco.viewer
+import numpy as np
+import time
+import math
+
+
+class QuadrotorSimulation:
+ def __init__(self):
+ """初始化四旋翼无人机仿真"""
+ # 使用简化的XML字符串，避免纹理问题
+ xml_string = self.create_minimal_quadrotor_xml()
+
+ # 从XML字符串加载模型
+ self.model = mujoco.MjModel.from_xml_string(xml_string)
+ print("✓ 模型加载成功")
+
+ # 创建仿真数据
+ self.data = mujoco.MjData(self.model)
+
+ # 获取执行器数量
+ self.n_actuators = self.model.nu
+ print(f"✓ 执行器数量: {self.n_actuators}")
+
+ # 设置初始控制输入
+ self.set_initial_control()
+
+ def create_minimal_quadrotor_xml(self):
+ """创建最简单的四旋翼无人机XML配置"""
+ xml_string = """
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+ return xml_string
+
+ def set_initial_control(self):
+ """设置初始控制输入"""
+ # 设置初始推力
+ hover_thrust = 500 # 悬停推力值
+ self.data.ctrl[:] = [hover_thrust] * self.n_actuators
+
+ def get_state(self):
+ """获取无人机状态"""
+ state = {
+ 'position': self.data.qpos[0:3].copy(),
+ 'orientation': self.data.qpos[3:7].copy(),
+ 'linear_velocity': self.data.qvel[0:3].copy(),
+ 'angular_velocity': self.data.qvel[3:6].copy(),
+ 'rotor_angles': self.data.qpos[7:11].copy(),
+ 'rotor_velocities': self.data.qvel[6:10].copy()
+ }
+ return state
+
+ def print_state(self):
+ """打印无人机状态"""
+ state = self.get_state()
+
+ print("\n" + "=" * 50)
+ print("四旋翼无人机状态:")
+ print("=" * 50)
+ print(f"位置: [{state['position'][0]:.3f}, {state['position'][1]:.3f}, {state['position'][2]:.3f}] m")
+ print(f"姿态四元数: [{state['orientation'][0]:.3f}, {state['orientation'][1]:.3f}, "
+ f"{state['orientation'][2]:.3f}, {state['orientation'][3]:.3f}]")
+ print(f"线速度: [{state['linear_velocity'][0]:.3f}, {state['linear_velocity'][1]:.3f}, "
+ f"{state['linear_velocity'][2]:.3f}] m/s")
+ print(f"角速度: [{state['angular_velocity'][0]:.3f}, {state['angular_velocity'][1]:.3f}, "
+ f"{state['angular_velocity'][2]:.3f}] rad/s")
+ print("=" * 50)
+
+ def apply_control(self, ctrl_values):
+ """应用控制输入"""
+ if len(ctrl_values) != self.n_actuators:
+ print(f"⚠ 警告：控制值数量应为{self.n_actuators}，使用默认值500")
+ ctrl_values = [500] * self.n_actuators
+
+ # 应用控制值
+ self.data.ctrl[:] = ctrl_values
+
+ def altitude_controller(self, target_z=1.5):
+ """高度控制器"""
+ # PID参数
+ Kp = 200.0 # 比例增益
+ Kd = 50.0 # 微分增益
+
+ # 获取当前状态
+ current_z = self.data.qpos[2]
+ current_vz = self.data.qvel[2]
+
+ # 计算误差
+ error_z = target_z - current_z
+ error_vz = 0 - current_vz
+
+ # PID控制
+ control_input = Kp * error_z + Kd * error_vz
+
+ # 基础推力
+ base_thrust = 500
+
+ # 计算推力
+ thrust = base_thrust + control_input
+
+ # 限制推力范围
+ thrust = np.clip(thrust, 400, 600)
+
+ # 应用到所有电机
+ ctrl_values = [thrust] * self.n_actuators
+ self.apply_control(ctrl_values)
+
+ return error_z, thrust
+
+ def position_controller(self, target_pos=[0, 0, 1.5]):
+ """位置控制器"""
+ # PID参数
+ Kp_pos = np.array([100.0, 100.0, 200.0])
+ Kd_pos = np.array([30.0, 30.0, 50.0])
+
+ # 获取当前状态
+ current_pos = self.data.qpos[0:3]
+ current_vel = self.data.qvel[0:3]
+
+ # 计算误差
+ pos_error = np.array(target_pos) - current_pos
+ vel_error = -current_vel
+
+ # 位置控制
+ pos_control = Kp_pos * pos_error + Kd_pos * vel_error
+
+ # 基础推力
+ base_thrust = 500
+
+ # 总推力
+ total_thrust = base_thrust + pos_control[2]
+
+ # 姿态控制
+ roll_control = -pos_control[1] * 0.02
+ pitch_control = pos_control[0] * 0.02
+
+ # 四旋翼混控
+ ctrl_values = [
+ total_thrust - pitch_control - roll_control, # 前右
+ total_thrust - pitch_control + roll_control, # 前左
+ total_thrust + pitch_control + roll_control, # 后左
+ total_thrust + pitch_control - roll_control # 后右
+ ]
+
+ # 限制推力范围
+ ctrl_values = np.clip(ctrl_values, 400, 600)
+
+ self.apply_control(ctrl_values)
+
+ return pos_error, ctrl_values
+
+ def run_simulation(self, duration=10.0, use_viewer=True, controller_type="altitude"):
+ """运行仿真"""
+ print(f"\n▶ 开始仿真，时长: {duration}秒")
+ print(f"▶ 控制器类型: {controller_type}")
+
+ if use_viewer:
+ print("▶ 使用可视化查看器 (按ESC退出)")
+ else:
+ print("▶ 无可视化模式")
+
+ # 记录数据
+ time_history = []
+ height_history = []
+ thrust_history = []
+
+ try:
+ if use_viewer:
+ with mujoco.viewer.launch_passive(self.model, self.data) as viewer:
+ # 设置相机
+ viewer.cam.azimuth = 180
+ viewer.cam.elevation = -20
+ viewer.cam.distance = 5.0
+ viewer.cam.lookat[:] = [0.0, 0.0, 1.0]
+
+ self.simulation_loop(viewer, duration, controller_type,
+ time_history, height_history, thrust_history)
+ else:
+ self.simulation_loop(None, duration, controller_type,
+ time_history, height_history, thrust_history)
+
+ except Exception as e:
+ print(f"⚠ 仿真错误: {e}")
+
+ # 分析数据
+ if time_history:
+ self.analyze_data(time_history, height_history, thrust_history)
+
+ def simulation_loop(self, viewer, duration, controller_type,
+ time_history, height_history, thrust_history):
+ """仿真循环"""
+ start_time = time.time()
+ last_print_time = time.time()
+ step_count = 0
+
+ while (viewer is None or (viewer and viewer.is_running())) and (time.time() - start_time) < duration:
+ step_start = time.time()
+ step_count += 1
+
+ # 应用控制器
+ if controller_type == "position":
+ # 移动目标点
+ t = self.data.time
+ target_x = 1.0 * math.sin(t * 0.5)
+ target_y = 1.0 * math.cos(t * 0.5)
+ target_z = 1.5 + 0.3 * math.sin(t * 0.3)
+
+ pos_error, thrusts = self.position_controller([target_x, target_y, target_z])
+ control_info = f"位置误差: [{pos_error[0]:.2f}, {pos_error[1]:.2f}, {pos_error[2]:.2f}] m"
+ else:
+ error_z, thrust = self.altitude_controller(1.5)
+ thrusts = [thrust] * 4
+ control_info = f"高度误差: {error_z:.2f} m"
+
+ # 记录数据
+ current_time = self.data.time
+ current_height = self.data.qpos[2]
+ time_history.append(current_time)
+ height_history.append(current_height)
+ thrust_history.append(np.mean(thrusts))
+
+ # 执行仿真步
+ mujoco.mj_step(self.model, self.data)
+
+ # 更新螺旋桨旋转（视觉效果）
+ rotor_speed = 80.0
+ for i in range(4):
+ self.data.qpos[7 + i] += rotor_speed * self.model.opt.timestep
+
+ # 更新查看器
+ if viewer:
+ viewer.sync()
+
+ # 打印状态信息
+ if time.time() - last_print_time > 1.0:
+ print(f"\n时间: {current_time:.1f}s | 高度: {current_height:.2f}m")
+ print(f"推力: {np.mean(thrusts):.0f} | {control_info}")
+ print(f"步数: {step_count}")
+ last_print_time = time.time()
+
+ # 控制仿真速度
+ elapsed = time.time() - step_start
+ sleep_time = self.model.opt.timestep - elapsed
+ if sleep_time > 0:
+ time.sleep(sleep_time)
+
+ def analyze_data(self, time_data, height_data, thrust_data):
+ """分析仿真数据"""
+ print("\n" + "=" * 50)
+ print("📊 仿真数据分析:")
+ print("=" * 50)
+
+ if not time_data:
+ print("无数据")
+ return
+
+ time_array = np.array(time_data)
+ height_array = np.array(height_data)
+ thrust_array = np.array(thrust_data)
+
+ print(f"总步数: {len(time_array)}")
+ print(f"仿真时长: {time_array[-1]:.2f} 秒")
+ print(f"平均高度: {np.mean(height_array):.3f} m")
+ print(f"高度稳定性: ±{np.std(height_array):.3f} m")
+ print(f"高度范围: [{np.min(height_array):.3f}, {np.max(height_array):.3f}] m")
+ print(f"平均推力: {np.mean(thrust_array):.0f}")
+ print(f"推力范围: [{np.min(thrust_array):.0f}, {np.max(thrust_array):.0f}]")
+
+ # 询问是否绘图
+ try:
+ plot = input("\n是否绘制图表? (y/n): ").strip().lower()
+ if plot == 'y':
+ self.plot_results(time_array, height_array, thrust_array)
+ except:
+ pass
+
+ def plot_results(self, time_data, height_data, thrust_data):
+ """绘制结果图表"""
+ try:
+ import matplotlib.pyplot as plt
+
+ fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))
+
+ # 高度图
+ ax1.plot(time_data, height_data, 'b-', linewidth=2, label='实际高度')
+ ax1.axhline(y=1.5, color='r', linestyle='--', alpha=0.7, label='目标高度')
+ ax1.fill_between(time_data, 1.45, 1.55, color='r', alpha=0.1)
+ ax1.set_xlabel('时间 (秒)')
+ ax1.set_ylabel('高度 (米)')
+ ax1.set_title('四旋翼无人机高度控制')
+ ax1.legend()
+ ax1.grid(True, alpha=0.3)
+
+ # 推力图
+ ax2.plot(time_data, thrust_data, 'g-', linewidth=2, label='平均推力')
+ ax2.axhline(y=500, color='orange', linestyle='--', alpha=0.7, label='悬停推力')
+ ax2.set_xlabel('时间 (秒)')
+ ax2.set_ylabel('推力')
+ ax2.set_title('电机推力变化')
+ ax2.legend()
+ ax2.grid(True, alpha=0.3)
+
+ plt.tight_layout()
+ plt.show()
+
+ except ImportError:
+ print("⚠ 需要安装matplotlib: pip install matplotlib")
+ except Exception as e:
+ print(f"⚠ 绘图错误: {e}")
+
+
+def main():
+ """主函数"""
+ print("🚁 MuJoCo 四旋翼无人机仿真系统")
+ print("=" * 50)
+
+ try:
+ # 创建仿真实例
+ print("正在初始化...")
+ sim = QuadrotorSimulation()
+ print("✅ 初始化完成")
+
+ # 用户选择
+ print("\n请选择仿真模式:")
+ print("1. 高度控制 (简单)")
+ print("2. 位置控制 (高级)")
+
+ choice = input("选择 (1/2): ").strip()
+ controller = "position" if choice == "2" else "altitude"
+
+ # 仿真时长
+ try:
+ duration = float(input("仿真时长(秒，默认10): ") or "10")
+ except:
+ duration = 10.0
+
+ # 是否使用可视化
+ viz = input("使用可视化? (y/n，默认y): ").strip().lower()
+ use_viz = viz != "n"
+
+ # 运行仿真
+ sim.run_simulation(
+ duration=duration,
+ use_viewer=use_viz,
+ controller_type=controller
+ )
+
+ except KeyboardInterrupt:
+ print("\n\n⏹ 仿真被用户中断")
+ except Exception as e:
+ print(f"\n❌ 错误: {e}")
+ import traceback
+ traceback.print_exc()
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/drone_simulation/model.xml b/src/drone_simulation/model.xml
new file mode 100644
index 0000000000..717a287e45
--- /dev/null
+++ b/src/drone_simulation/model.xml
@@ -0,0 +1,164 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
From 74ec5e5f15c61f1e26d2b9154bd5ab3d3ce089fb Mon Sep 17 00:00:00 2001
From: ZKX34 <3442977470@qq.com>
Date: Mon, 22 Dec 2025 21:18:29 +0800
Subject: [PATCH 2/7] =?UTF-8?q?=E5=BB=BA=E7=AB=8B=E4=B8=80=E4=B8=AA?=
=?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E6=A8=A1=E5=9E=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/drone_simulation/main.py | 511 +++++------------------------------
1 file changed, 62 insertions(+), 449 deletions(-)
diff --git a/src/drone_simulation/main.py b/src/drone_simulation/main.py
index e13322b3e8..b6804d05bc 100644
--- a/src/drone_simulation/main.py
+++ b/src/drone_simulation/main.py
@@ -1,473 +1,86 @@
-"""
-MuJoCo 四旋翼无人机仿真 - 修复纹理错误版本
-"""
-
+# simple_visible_drone.py
import mujoco
import mujoco.viewer
import numpy as np
import time
-import math
-
-
-class QuadrotorSimulation:
- def __init__(self):
- """初始化四旋翼无人机仿真"""
- # 使用简化的XML字符串，避免纹理问题
- xml_string = self.create_minimal_quadrotor_xml()
-
- # 从XML字符串加载模型
- self.model = mujoco.MjModel.from_xml_string(xml_string)
- print("✓ 模型加载成功")
-
- # 创建仿真数据
- self.data = mujoco.MjData(self.model)
-
- # 获取执行器数量
- self.n_actuators = self.model.nu
- print(f"✓ 执行器数量: {self.n_actuators}")
-
- # 设置初始控制输入
- self.set_initial_control()
-
- def create_minimal_quadrotor_xml(self):
- """创建最简单的四旋翼无人机XML配置"""
- xml_string = """
-
-
-
-
-
-
+# 最小化但可见的模型
+MJCF_MODEL = """
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+"""
-
-
-
-
-
-
-
-
-
-"""
- return xml_string
-
- def set_initial_control(self):
- """设置初始控制输入"""
- # 设置初始推力
- hover_thrust = 500 # 悬停推力值
- self.data.ctrl[:] = [hover_thrust] * self.n_actuators
-
- def get_state(self):
- """获取无人机状态"""
- state = {
- 'position': self.data.qpos[0:3].copy(),
- 'orientation': self.data.qpos[3:7].copy(),
- 'linear_velocity': self.data.qvel[0:3].copy(),
- 'angular_velocity': self.data.qvel[3:6].copy(),
- 'rotor_angles': self.data.qpos[7:11].copy(),
- 'rotor_velocities': self.data.qvel[6:10].copy()
- }
- return state
-
- def print_state(self):
- """打印无人机状态"""
- state = self.get_state()
-
- print("\n" + "=" * 50)
- print("四旋翼无人机状态:")
- print("=" * 50)
- print(f"位置: [{state['position'][0]:.3f}, {state['position'][1]:.3f}, {state['position'][2]:.3f}] m")
- print(f"姿态四元数: [{state['orientation'][0]:.3f}, {state['orientation'][1]:.3f}, "
- f"{state['orientation'][2]:.3f}, {state['orientation'][3]:.3f}]")
- print(f"线速度: [{state['linear_velocity'][0]:.3f}, {state['linear_velocity'][1]:.3f}, "
- f"{state['linear_velocity'][2]:.3f}] m/s")
- print(f"角速度: [{state['angular_velocity'][0]:.3f}, {state['angular_velocity'][1]:.3f}, "
- f"{state['angular_velocity'][2]:.3f}] rad/s")
- print("=" * 50)
-
- def apply_control(self, ctrl_values):
- """应用控制输入"""
- if len(ctrl_values) != self.n_actuators:
- print(f"⚠ 警告：控制值数量应为{self.n_actuators}，使用默认值500")
- ctrl_values = [500] * self.n_actuators
-
- # 应用控制值
- self.data.ctrl[:] = ctrl_values
-
- def altitude_controller(self, target_z=1.5):
- """高度控制器"""
- # PID参数
- Kp = 200.0 # 比例增益
- Kd = 50.0 # 微分增益
-
- # 获取当前状态
- current_z = self.data.qpos[2]
- current_vz = self.data.qvel[2]
-
- # 计算误差
- error_z = target_z - current_z
- error_vz = 0 - current_vz
-
- # PID控制
- control_input = Kp * error_z + Kd * error_vz
-
- # 基础推力
- base_thrust = 500
-
- # 计算推力
- thrust = base_thrust + control_input
-
- # 限制推力范围
- thrust = np.clip(thrust, 400, 600)
-
- # 应用到所有电机
- ctrl_values = [thrust] * self.n_actuators
- self.apply_control(ctrl_values)
-
- return error_z, thrust
-
- def position_controller(self, target_pos=[0, 0, 1.5]):
- """位置控制器"""
- # PID参数
- Kp_pos = np.array([100.0, 100.0, 200.0])
- Kd_pos = np.array([30.0, 30.0, 50.0])
-
- # 获取当前状态
- current_pos = self.data.qpos[0:3]
- current_vel = self.data.qvel[0:3]
-
- # 计算误差
- pos_error = np.array(target_pos) - current_pos
- vel_error = -current_vel
-
- # 位置控制
- pos_control = Kp_pos * pos_error + Kd_pos * vel_error
-
- # 基础推力
- base_thrust = 500
-
- # 总推力
- total_thrust = base_thrust + pos_control[2]
-
- # 姿态控制
- roll_control = -pos_control[1] * 0.02
- pitch_control = pos_control[0] * 0.02
-
- # 四旋翼混控
- ctrl_values = [
- total_thrust - pitch_control - roll_control, # 前右
- total_thrust - pitch_control + roll_control, # 前左
- total_thrust + pitch_control + roll_control, # 后左
- total_thrust + pitch_control - roll_control # 后右
- ]
-
- # 限制推力范围
- ctrl_values = np.clip(ctrl_values, 400, 600)
-
- self.apply_control(ctrl_values)
-
- return pos_error, ctrl_values
-
- def run_simulation(self, duration=10.0, use_viewer=True, controller_type="altitude"):
- """运行仿真"""
- print(f"\n▶ 开始仿真，时长: {duration}秒")
- print(f"▶ 控制器类型: {controller_type}")
-
- if use_viewer:
- print("▶ 使用可视化查看器 (按ESC退出)")
- else:
- print("▶ 无可视化模式")
-
- # 记录数据
- time_history = []
- height_history = []
- thrust_history = []
-
- try:
- if use_viewer:
- with mujoco.viewer.launch_passive(self.model, self.data) as viewer:
- # 设置相机
- viewer.cam.azimuth = 180
- viewer.cam.elevation = -20
- viewer.cam.distance = 5.0
- viewer.cam.lookat[:] = [0.0, 0.0, 1.0]
-
- self.simulation_loop(viewer, duration, controller_type,
- time_history, height_history, thrust_history)
- else:
- self.simulation_loop(None, duration, controller_type,
- time_history, height_history, thrust_history)
-
- except Exception as e:
- print(f"⚠ 仿真错误: {e}")
-
- # 分析数据
- if time_history:
- self.analyze_data(time_history, height_history, thrust_history)
-
- def simulation_loop(self, viewer, duration, controller_type,
- time_history, height_history, thrust_history):
- """仿真循环"""
- start_time = time.time()
- last_print_time = time.time()
- step_count = 0
-
- while (viewer is None or (viewer and viewer.is_running())) and (time.time() - start_time) < duration:
- step_start = time.time()
- step_count += 1
-
- # 应用控制器
- if controller_type == "position":
- # 移动目标点
- t = self.data.time
- target_x = 1.0 * math.sin(t * 0.5)
- target_y = 1.0 * math.cos(t * 0.5)
- target_z = 1.5 + 0.3 * math.sin(t * 0.3)
-
- pos_error, thrusts = self.position_controller([target_x, target_y, target_z])
- control_info = f"位置误差: [{pos_error[0]:.2f}, {pos_error[1]:.2f}, {pos_error[2]:.2f}] m"
- else:
- error_z, thrust = self.altitude_controller(1.5)
- thrusts = [thrust] * 4
- control_info = f"高度误差: {error_z:.2f} m"
-
- # 记录数据
- current_time = self.data.time
- current_height = self.data.qpos[2]
- time_history.append(current_time)
- height_history.append(current_height)
- thrust_history.append(np.mean(thrusts))
-
- # 执行仿真步
- mujoco.mj_step(self.model, self.data)
-
- # 更新螺旋桨旋转（视觉效果）
- rotor_speed = 80.0
- for i in range(4):
- self.data.qpos[7 + i] += rotor_speed * self.model.opt.timestep
-
- # 更新查看器
- if viewer:
- viewer.sync()
-
- # 打印状态信息
- if time.time() - last_print_time > 1.0:
- print(f"\n时间: {current_time:.1f}s | 高度: {current_height:.2f}m")
- print(f"推力: {np.mean(thrusts):.0f} | {control_info}")
- print(f"步数: {step_count}")
- last_print_time = time.time()
-
- # 控制仿真速度
- elapsed = time.time() - step_start
- sleep_time = self.model.opt.timestep - elapsed
- if sleep_time > 0:
- time.sleep(sleep_time)
-
- def analyze_data(self, time_data, height_data, thrust_data):
- """分析仿真数据"""
- print("\n" + "=" * 50)
- print("📊 仿真数据分析:")
- print("=" * 50)
-
- if not time_data:
- print("无数据")
- return
-
- time_array = np.array(time_data)
- height_array = np.array(height_data)
- thrust_array = np.array(thrust_data)
-
- print(f"总步数: {len(time_array)}")
- print(f"仿真时长: {time_array[-1]:.2f} 秒")
- print(f"平均高度: {np.mean(height_array):.3f} m")
- print(f"高度稳定性: ±{np.std(height_array):.3f} m")
- print(f"高度范围: [{np.min(height_array):.3f}, {np.max(height_array):.3f}] m")
- print(f"平均推力: {np.mean(thrust_array):.0f}")
- print(f"推力范围: [{np.min(thrust_array):.0f}, {np.max(thrust_array):.0f}]")
-
- # 询问是否绘图
- try:
- plot = input("\n是否绘制图表? (y/n): ").strip().lower()
- if plot == 'y':
- self.plot_results(time_array, height_array, thrust_array)
- except:
- pass
-
- def plot_results(self, time_data, height_data, thrust_data):
- """绘制结果图表"""
- try:
- import matplotlib.pyplot as plt
-
- fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))
-
- # 高度图
- ax1.plot(time_data, height_data, 'b-', linewidth=2, label='实际高度')
- ax1.axhline(y=1.5, color='r', linestyle='--', alpha=0.7, label='目标高度')
- ax1.fill_between(time_data, 1.45, 1.55, color='r', alpha=0.1)
- ax1.set_xlabel('时间 (秒)')
- ax1.set_ylabel('高度 (米)')
- ax1.set_title('四旋翼无人机高度控制')
- ax1.legend()
- ax1.grid(True, alpha=0.3)
-
- # 推力图
- ax2.plot(time_data, thrust_data, 'g-', linewidth=2, label='平均推力')
- ax2.axhline(y=500, color='orange', linestyle='--', alpha=0.7, label='悬停推力')
- ax2.set_xlabel('时间 (秒)')
- ax2.set_ylabel('推力')
- ax2.set_title('电机推力变化')
- ax2.legend()
- ax2.grid(True, alpha=0.3)
- plt.tight_layout()
- plt.show()
+def main():
+ print("正在启动无人机仿真...")
+ print("按ESC退出窗口")
+ print("等待3秒...")
- except ImportError:
- print("⚠ 需要安装matplotlib: pip install matplotlib")
- except Exception as e:
- print(f"⚠ 绘图错误: {e}")
+ # 直接从字符串加载模型
+ model = mujoco.MjModel.from_xml_string(MJCF_MODEL)
+ data = mujoco.MjData(model)
+ # 等待3秒
+ time.sleep(3)
-def main():
- """主函数"""
- print("🚁 MuJoCo 四旋翼无人机仿真系统")
- print("=" * 50)
+ print("开始飞行演示...")
- try:
- # 创建仿真实例
- print("正在初始化...")
- sim = QuadrotorSimulation()
- print("✅ 初始化完成")
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ # 设置相机视角
+ viewer.cam.lookat[:] = [0, 0, 1]
+ viewer.cam.distance = 8.0
+ viewer.cam.azimuth = 45
+ viewer.cam.elevation = -30
- # 用户选择
- print("\n请选择仿真模式:")
- print("1. 高度控制 (简单)")
- print("2. 位置控制 (高级)")
+ t = 0
+ while viewer.is_running() and t < 20: # 运行20秒
+ # 简单的上下浮动
+ force_z = 20 * np.sin(t * 2) + 50
- choice = input("选择 (1/2): ").strip()
- controller = "position" if choice == "2" else "altitude"
+ # 应用力
+ data.qfrc_applied[2] = force_z
- # 仿真时长
- try:
- duration = float(input("仿真时长(秒，默认10): ") or "10")
- except:
- duration = 10.0
+ # 缓慢旋转
+ data.qfrc_applied[5] = 5 * np.sin(t * 0.5)
- # 是否使用可视化
- viz = input("使用可视化? (y/n，默认y): ").strip().lower()
- use_viz = viz != "n"
+ mujoco.mj_step(model, data)
+ viewer.sync()
- # 运行仿真
- sim.run_simulation(
- duration=duration,
- use_viewer=use_viz,
- controller_type=controller
- )
+ t += 0.01
+ time.sleep(0.01)
- except KeyboardInterrupt:
- print("\n\n⏹ 仿真被用户中断")
- except Exception as e:
- print(f"\n❌ 错误: {e}")
- import traceback
- traceback.print_exc()
+ print("仿真结束！")
if __name__ == "__main__":
From 222c8ee2ea82e32b40c19e8a41ecefa40d8f9123 Mon Sep 17 00:00:00 2001
From: ZKX34 <3442977470@qq.com>
Date: Tue, 23 Dec 2025 15:18:47 +0800
Subject: [PATCH 3/7] =?UTF-8?q?=E5=BB=BA=E7=AB=8B=E4=B8=80=E4=B8=AA?=
=?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E6=A8=A1=E5=9E=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/drone_simulation/main.py | 45 ++++++++++++++++++++++++++++--------
1 file changed, 35 insertions(+), 10 deletions(-)
diff --git a/src/drone_simulation/main.py b/src/drone_simulation/main.py
index b6804d05bc..1976d41ec0 100644
--- a/src/drone_simulation/main.py
+++ b/src/drone_simulation/main.py
@@ -3,6 +3,36 @@
import mujoco.viewer
import numpy as np
import time
+import os
+import glob
+
+# 定义日志文件路径（可根据实际情况修改）
+LOG_FILE_PATHS = [
+ "mujoco.log",
+ "drone_simulation.log",
+ "./logs/*.log"
+]
+
+
+def delete_log_files():
+ """删除指定的日志文件（无控制台输出）"""
+ deleted_count = 0
+ for path in LOG_FILE_PATHS:
+ if "*" in path:
+ for file in glob.glob(path):
+ try:
+ os.remove(file)
+ deleted_count += 1
+ except (FileNotFoundError, PermissionError):
+ continue
+ else:
+ if os.path.exists(path):
+ try:
+ os.remove(path)
+ deleted_count += 1
+ except (FileNotFoundError, PermissionError):
+ continue
+
# 最小化但可见的模型
MJCF_MODEL = """
@@ -43,19 +73,16 @@
def main():
- print("正在启动无人机仿真...")
- print("按ESC退出窗口")
- print("等待3秒...")
+ # 删除日志文件（无输出）
+ delete_log_files()
- # 直接从字符串加载模型
+ # 加载模型和数据
model = mujoco.MjModel.from_xml_string(MJCF_MODEL)
data = mujoco.MjData(model)
- # 等待3秒
+ # 等待3秒（无提示）
time.sleep(3)
- print("开始飞行演示...")
-
with mujoco.viewer.launch_passive(model, data) as viewer:
# 设置相机视角
viewer.cam.lookat[:] = [0, 0, 1]
@@ -64,7 +91,7 @@ def main():
viewer.cam.elevation = -30
t = 0
- while viewer.is_running() and t < 20: # 运行20秒
+ while viewer.is_running() and t < 20:
# 简单的上下浮动
force_z = 20 * np.sin(t * 2) + 50
@@ -80,8 +107,6 @@ def main():
t += 0.01
time.sleep(0.01)
- print("仿真结束！")
-
if __name__ == "__main__":
main()
\ No newline at end of file
From 4fe6a0ee407a35c8b1fd89ec829139aa29fedd84 Mon Sep 17 00:00:00 2001
From: ZKX34 <3442977470@qq.com>
Date: Tue, 23 Dec 2025 18:57:12 +0800
Subject: [PATCH 4/7] =?UTF-8?q?=E5=BB=BA=E7=AB=8B=E4=B8=80=E4=B8=AA?=
=?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E6=A8=A1=E5=9E=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/drone_simulation/main.py | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/drone_simulation/main.py b/src/drone_simulation/main.py
index 1976d41ec0..2874d4690a 100644
--- a/src/drone_simulation/main.py
+++ b/src/drone_simulation/main.py
@@ -109,4 +109,5 @@ def main():
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()
+#qwerty
\ No newline at end of file
From 4995e85b91407a012f4fcd5962b73ae4fe6c520d Mon Sep 17 00:00:00 2001
From: ZKX34 <3442977470@qq.com>
Date: Wed, 24 Dec 2025 20:34:58 +0800
Subject: [PATCH 5/7] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=97=A5=E5=BF=97?=
=?UTF-8?q?=E5=92=8C=E7=94=9F=E6=88=90=E6=96=87=E4=BB=B6=EF=BC=8C=E4=BF=9D?=
=?UTF-8?q?=E7=95=99xml=E6=96=87=E4=BB=B6=EF=BC=8C=E6=B7=BB=E5=8A=A0.gitig?=
=?UTF-8?q?nore?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.gitignore | Bin 0 -> 382 bytes
src/drone_simulation/MUJOCO_LOG.TXT | 3 ---
2 files changed, 3 deletions(-)
delete mode 100644 src/drone_simulation/MUJOCO_LOG.TXT
diff --git a/.gitignore b/.gitignore
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..357efa4d043105b21b6f83d6e7305adabfbae237 100644
GIT binary patch
literal 382
zcmZvY%?`mp6ot~L?(BV
zIp^MQ&W!&^B91cFn8Sh#2Qfs@
Date: Wed, 24 Dec 2025 20:35:33 +0800
Subject: [PATCH 6/7] =?UTF-8?q?=E9=A1=B9=E7=9B=AE=E6=B8=85=E7=90=86?=
=?UTF-8?q?=EF=BC=9A=E5=88=A0=E9=99=A4TXT/mjb/stl=E6=96=87=E4=BB=B6?=
=?UTF-8?q?=EF=BC=8C=E4=BF=9D=E7=95=99xml?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.gitignore | Bin 382 -> 126 bytes
requirements.txt | 3 ++-
2 files changed, 2 insertions(+), 1 deletion(-)
diff --git a/.gitignore b/.gitignore
index 357efa4d043105b21b6f83d6e7305adabfbae237..daba3febf6dd5b28f9852df7c8a9a8b1e0ddd154 100644
GIT binary patch
delta 73
zcmeyzRA==6pE832!(IQ#Qay$WhFpdm2A=;4K`GP90~L?(BV
zIp^MQ&W!&^B91cFn8Sh#2Qfs@=2.3.0
\ No newline at end of file
From 3b63f0c2c8e38bb5450ce949e8dd9aa42c66ae73 Mon Sep 17 00:00:00 2001
From: ZKX34 <3442977470@qq.com>
Date: Wed, 24 Dec 2025 20:56:14 +0800
Subject: [PATCH 7/7] =?UTF-8?q?=E5=BB=BA=E7=AB=8B=E4=B8=80=E4=B8=AA?=
=?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E6=A8=A1=E5=9E=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/drone_simulation/main.py | 1 -
1 file changed, 1 deletion(-)
diff --git a/src/drone_simulation/main.py b/src/drone_simulation/main.py
index 2874d4690a..78f334b53d 100644
--- a/src/drone_simulation/main.py
+++ b/src/drone_simulation/main.py
@@ -110,4 +110,3 @@ def main():
if __name__ == "__main__":
main()
-#qwerty
\ No newline at end of file