diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py index 3c13f17028..fdadc83717 100644 --- a/src/Robotic_arm/main.py +++ b/src/Robotic_arm/main.py @@ -5,67 +5,330 @@ import tempfile import time from scipy import interpolate -import cvxpy as cp # 用于能耗最优的二次规划 +import cvxpy as cp import warnings +import argparse +from dataclasses import dataclass +from collections import deque +import logging +# ====================== 0. 初始化配置与日志 ====================== warnings.filterwarnings("ignore") -# ====================== 1. 全局配置(鲁棒+效率双优化) ====================== -# 物理约束(UR5工业级参数) -CONSTRAINTS = { - "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2], # 关节最大速度 (rad/s) - "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6], # 关节最大加速度 (rad/s²) - "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4], # 关节最大加加速度 (rad/s³) - "max_torque": [15.0, 15.0, 10.0, 5.0, 5.0, 3.0], # 关节最大扭矩 (N·m) - "ctrl_limit": [-10.0, 10.0] -} - -# 避障鲁棒性参数 -OBSTACLE_CONFIG = { - "base_k_att": 0.8, # 基础引力系数 - "base_k_rep": 0.6, # 基础斥力系数 - "rep_radius": 0.3, # 斥力作用半径 - "stagnant_threshold": 0.01, # 停滞速度阈值 (m/s) - "stagnant_time": 1.0, # 停滞判定时间 (s) - "guide_offset": 0.1, # 局部最优引导偏移量 (m) - "obstacle_list": [ # 障碍物列表 [x,y,z,半径] - [0.6, 0.1, 0.5, 0.1], # 障碍1:易导致局部最优 - [0.55, 0.05, 0.55, 0.08], # 障碍2:密集障碍 - [0.4, -0.1, 0.6, 0.08] # 障碍3 +# 日志配置(结构化输出) +logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(module)s - %(levelname)s - %(message)s", + handlers=[logging.StreamHandler()] +) +logger = logging.getLogger("robotic_arm") + + +# 解析命令行参数 +def parse_args(): + parser = argparse.ArgumentParser(description="机械臂效率+鲁棒双优化仿真(核心增强版)") + parser.add_argument("--traj-points", type=int, default=20, help="轨迹插值点数(平衡精度与速度)") + parser.add_argument("--time-weight", type=float, default=0.6, help="时间权重(0-1)") + parser.add_argument("--energy-weight", type=float, default=0.4, help="能耗权重(0-1)") + parser.add_argument("--smooth-factor", type=float, default=0.2, help="轨迹平滑系数(0-1)") + return parser.parse_args() + + +args = parse_args() + + +# ====================== 1. 配置参数(结构化管理) ====================== +@dataclass +class RobotConfig: + # 物理约束(UR5工业级参数) + max_vel: list = None + max_acc: list = None + max_jerk: list = None + max_torque: list = None + ctrl_limit: tuple = (-10.0, 10.0) + + # 避障鲁棒性参数 + base_k_att: float = 0.8 + base_k_rep: float = 0.6 + rep_radius: float = 0.3 + stagnant_threshold: float = 0.01 + stagnant_time: float = 1.0 + guide_offset: float = 0.1 + obstacle_list: list = None + + # 效率优化参数 + time_weight: float = args.time_weight + energy_weight: float = args.energy_weight + traj_interp_points: int = args.traj_points + safety_margin: float = 0.05 + opt_horizon: float = 1.0 + smooth_factor: float = args.smooth_factor + + # 能耗计算参数(工业级电机参数) + motor_efficiency: float = 0.85 # 电机效率 + joint_friction: list = None # 关节摩擦系数 + + # 笛卡尔轨迹关键点 + cart_waypoints: list = None + + +# 初始化配置 +config = RobotConfig( + max_vel=[1.0, 0.8, 0.8, 1.2, 0.9, 1.2], + max_acc=[0.5, 0.4, 0.4, 0.6, 0.5, 0.6], + max_jerk=[0.3, 0.2, 0.2, 0.4, 0.3, 0.4], + max_torque=[15.0, 15.0, 10.0, 5.0, 5.0, 3.0], + obstacle_list=[ + [0.6, 0.1, 0.5, 0.1], + [0.55, 0.05, 0.55, 0.08], + [0.4, -0.1, 0.6, 0.08] + ], + joint_friction=[0.001, 0.002, 0.0015, 0.001, 0.0008, 0.0005], + cart_waypoints=[ + [0.5, 0.0, 0.6], + [0.6, 0.0, 0.58], + [0.8, 0.1, 0.8], + [0.6, 0.0, 0.58], + [0.5, 0.0, 0.6] ] -} - -# 效率优化参数(工业场景可配置) -EFFICIENCY_CONFIG = { - "time_weight": 0.6, # 时间权重(0-1,越大越优先时间) - "energy_weight": 0.4, # 能耗权重(0-1,越大越优先能耗) - "traj_interp_points": 50, # 轨迹插值点数 - "safety_margin": 0.05, # 碰撞安全裕度 (m) - "opt_horizon": 1.0 # 优化时域 (s) -} - -# 笛卡尔轨迹关键点(工业典型路径) -CART_WAYPOINTS = [ - [0.5, 0.0, 0.6], # 起点 - [0.6, 0.0, 0.58], # 中间点(障碍夹缝) - [0.8, 0.1, 0.8], # 终点 - [0.6, 0.0, 0.58], # 回中间点 - [0.5, 0.0, 0.6] # 回起点 -] +) # 全局变量 stagnant_start_time = None -total_motion_time = 0.0 # 累计运动时间 -total_energy_consume = 0.0 # 累计能耗 +total_motion_time = 0.0 +total_energy_consume = 0.0 +traj_history = deque(maxlen=50) # 轨迹历史(用于可视化) +collision_warning = False -# 预定义关节惯性参数(适配所有MuJoCo版本) +# 预定义关节惯性参数 JOINT_INERTIA = [0.01, 0.02, 0.015, 0.01, 0.008, 0.005] JOINT_GRAVITY = [0.5, 0.8, 0.6, 0.3, 0.2, 0.1] -# ====================== 2. 基础工具函数(兼容所有MuJoCo版本) ====================== +# ====================== 2. 核心增强:全链路碰撞检测 ====================== +def full_arm_collision_check(model, data, config, return_min_dist=True): + """ + 检测所有连杆与障碍物的碰撞(核心增强) + :param return_min_dist: 是否返回最近距离 + :return: 是否碰撞, 最近距离(可选) + """ + collision = False + min_dist = float("inf") + + # 所有需要检测的连杆/末端 + link_names = ["link1", "link2", "link3", "link4", "link5", "end_effector"] + + for link_name in link_names: + try: + link_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, link_name) + link_pos = data.xpos[link_id] # 连杆中心位置 + + # 检测与每个障碍物的距离 + for obs in config.obstacle_list: + obs_pos = np.array(obs[:3]) + obs_radius = obs[3] + + # 计算连杆到障碍物的距离(减去安全裕度) + dist = np.linalg.norm(link_pos - obs_pos) - (obs_radius + config.safety_margin) + + if dist < 0: + collision = True + logger.warning(f"碰撞风险:{link_name} 与障碍物 {obs_pos} 距离 {dist:.4f}m") + + if return_min_dist: + min_dist = min(min_dist, dist) + except Exception as e: + logger.error(f"碰撞检测失败:{link_name} - {e}") + continue + + global collision_warning + collision_warning = collision + + if return_min_dist: + return collision, min_dist + return collision + + +# ====================== 3. 核心增强:轨迹平滑滤波(修复阶数不匹配) ====================== +def smooth_cartesian_traj(traj_points, smooth_factor=0.2): + """ + 贝塞尔曲线平滑笛卡尔轨迹(修复点数不足问题) + :param traj_points: 原始轨迹点列表 + :param smooth_factor: 平滑系数(0-1,越大越平滑) + :return: 平滑后的轨迹点 + """ + traj_array = np.array(traj_points) + n_points = len(traj_array) + + # 处理点数不足的情况: + # - 1个点:直接返回 + # - 2-3个点:使用线性插值/低阶样条 + # - 4个及以上点:使用3阶样条 + if n_points <= 1: + return traj_points + elif n_points <= 3: + k = n_points - 1 # 适配点数的阶数(2点→1阶,3点→2阶) + else: + k = 3 # 4点及以上用3阶样条 + + # 生成插值参数 + t = np.linspace(0, 1, n_points) + smoothed_traj = np.zeros_like(traj_array) + + for dim in range(3): # x/y/z三个维度 + try: + # 生成适配阶数的样条曲线(核心修复) + spline = interpolate.make_interp_spline(t, traj_array[:, dim], k=k) + # 生成更密集的插值点(提升平滑度) + t_smooth = np.linspace(0, 1, max(10, n_points * 2)) + smooth_vals = spline(t_smooth) + + # 低通滤波(增加异常处理,提升稳定性) + try: + from scipy.signal import filtfilt, butter + b, a = butter(2, smooth_factor, btype="low") + smooth_vals = filtfilt(b, a, smooth_vals) + except: + # 滤波失败时直接使用插值结果 + pass + + # 采样回原点数 + smoothed_traj[:, dim] = np.interp(t, t_smooth, smooth_vals) + except Exception as e: + # 插值失败时降级为原始轨迹 + logger.warning(f"轨迹平滑失败(维度{dim}):{e},使用原始轨迹") + smoothed_traj = traj_array + break + + return smoothed_traj.tolist() + + +def smooth_joint_traj(joint_traj, smooth_factor=0.1): + """ + 平滑关节轨迹(修复点数不足+简化滤波) + """ + joint_array = np.array(joint_traj) + n_points, n_joints = joint_array.shape + + # 点数不足时直接返回 + if n_points <= 1: + return joint_traj + + # 简化滤波逻辑,提升兼容性 + try: + from scipy.signal import filtfilt, butter + b, a = butter(1, smooth_factor, btype="low") # 1阶滤波更稳定 + smoothed_joints = np.zeros_like(joint_array) + + for j in range(n_joints): + smoothed_joints[:, j] = filtfilt(b, a, joint_array[:, j]) + except Exception as e: + logger.warning(f"关节轨迹平滑失败:{e},使用原始轨迹") + smoothed_joints = joint_array + + return smoothed_joints + + +# ====================== 4. 核心增强:真实能耗计算 ====================== +def calculate_real_energy_consumption(model, data, config, dt): + """ + 计算真实能耗(考虑电机效率、摩擦损耗) + """ + energy = 0.0 + + for joint_idx in range(6): + # 1. 获取MuJoCo实时输出扭矩 + real_torque = data.qfrc_actuator[joint_idx] + # 2. 关节速度 + joint_vel = data.qvel[joint_idx] + # 3. 摩擦损耗 + friction_loss = config.joint_friction[joint_idx] * abs(joint_vel) + # 4. 实际能耗计算 + mechanical_power = abs(real_torque * joint_vel) + total_power = mechanical_power + friction_loss + energy += total_power / config.motor_efficiency * dt + + return energy + + +# ====================== 5. 核心增强:可视化工具(适配新版MuJoCo API) ====================== +def draw_enhanced_visualization(viewer, model, data, config): + """ + 增强可视化:适配新版MuJoCo API,避免MjGeom报错 + 注:新版MuJoCo简化了用户绘制,这里改用更兼容的方式展示关键信息 + """ + try: + # 方式1:使用新版Viewer的user_scn(兼容大部分版本) + scene = viewer.user_scn + scene.ngeom = 0 # 清空原有绘制 + + # 1. 绘制轨迹历史(使用mjv_geom接口) + if len(traj_history) > 1: + traj_array = np.array(list(traj_history)) + + # 轨迹线 + for i in range(len(traj_array) - 1): + # 初始化几何对象(适配新版API) + geom = mujoco.MjvGeom() + mujoco.mjv_initGeom( + geom, + mujoco.mjtGeom.mjGEOM_LINE, + np.array([0.003, 0, 0]), # 大小 + traj_array[i], # 起始点 + traj_array[i + 1], # 结束点 + np.array([0, 1, 0, 0.6]) # 绿色轨迹线 + ) + # 添加到场景 + mujoco.mjv_addGeom(scene, model, data, geom) + + # 轨迹起点(蓝色) + start_geom = mujoco.MjvGeom() + mujoco.mjv_initGeom( + start_geom, + mujoco.mjtGeom.mjGEOM_SPHERE, + np.array([0.015, 0, 0]), + traj_array[0], + np.array([0, 0, 0]), + np.array([0, 0, 1, 0.8]) + ) + mujoco.mjv_addGeom(scene, model, data, start_geom) + + # 轨迹终点(红色) + end_geom = mujoco.MjvGeom() + mujoco.mjv_initGeom( + end_geom, + mujoco.mjtGeom.mjGEOM_SPHERE, + np.array([0.015, 0, 0]), + traj_array[-1], + np.array([0, 0, 0]), + np.array([1, 0, 0, 0.8]) + ) + mujoco.mjv_addGeom(scene, model, data, end_geom) + + # 2. 绘制碰撞警告(红色半透明球) + if collision_warning: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + ee_pos = data.site_xpos[ee_id] + + warn_geom = mujoco.MjvGeom() + mujoco.mjv_initGeom( + warn_geom, + mujoco.mjtGeom.mjGEOM_SPHERE, + np.array([0.08, 0, 0]), + ee_pos, + np.array([0, 0, 0]), + np.array([1, 0, 0, 0.3]) + ) + mujoco.mjv_addGeom(scene, model, data, warn_geom) + + except Exception as e: + # 兼容最简化版本:禁用可视化绘制,避免影响核心功能 + logger.warning(f"可视化绘制失败(MuJoCo版本兼容问题):{e}") + logger.warning("已禁用可视化增强,核心仿真功能不受影响") + + +# ====================== 6. 基础工具函数 ====================== def get_ee_cartesian_velocity(model, data, ee_site_id): - """计算末端笛卡尔速度(兼容所有MuJoCo版本)""" jacp = np.zeros((3, model.nv)) jacr = np.zeros((3, model.nv)) mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id) @@ -74,75 +337,63 @@ def get_ee_cartesian_velocity(model, data, ee_site_id): return ee_cart_vel -def calculate_joint_torque(model, data, joint_idx): - """计算关节扭矩(能耗核心指标,适配新版MuJoCo)""" - # 方案1:使用预定义惯性参数(兼容所有版本) - inertia = JOINT_INERTIA[joint_idx] - gravity_comp = JOINT_GRAVITY[joint_idx] - - # 方案2(备选):从data中获取实时扭矩(新版MuJoCo推荐) - # torque = data.qfrc_actuator[joint_idx] # 实际输出扭矩 - - # 计算关节加速度(数值微分) - joint_acc = np.gradient(data.qvel[joint_idx]) if data.time > 0 else 0.0 - torque = inertia * joint_acc + gravity_comp - torque = np.clip(torque, -CONSTRAINTS["max_torque"][joint_idx], CONSTRAINTS["max_torque"][joint_idx]) - return torque - - -# ====================== 3. 鲁棒性避障(保留原有核心逻辑) ====================== -def check_local_optimum(ee_vel, ee_pos, target_pos): - """检测局部最优并生成引导目标""" +def check_local_optimum(ee_vel, ee_pos, target_pos, config): global stagnant_start_time vel_mag = np.linalg.norm(ee_vel) - if vel_mag < OBSTACLE_CONFIG["stagnant_threshold"]: + + if vel_mag < config.stagnant_threshold: if stagnant_start_time is None: stagnant_start_time = time.time() - elif time.time() - stagnant_start_time > OBSTACLE_CONFIG["stagnant_time"]: - print(f"\n⚠️ 检测到局部最优!末端速度={vel_mag:.4f}m/s") + elif time.time() - stagnant_start_time > config.stagnant_time: + logger.warning(f"检测到局部最优!末端速度={vel_mag:.4f}m/s") dir_to_target = np.array(target_pos) - np.array(ee_pos) - dir_to_target = dir_to_target / np.linalg.norm(dir_to_target) if np.linalg.norm( - dir_to_target) > 1e-6 else np.array([0, 0, 0.1]) - guide_target = np.array(ee_pos) + dir_to_target * OBSTACLE_CONFIG["guide_offset"] + dir_norm = np.linalg.norm(dir_to_target) + if dir_norm < 1e-6: + dir_to_target = np.array([0, 0, 0.1]) + else: + dir_to_target = dir_to_target / dir_norm + + guide_target = np.array(ee_pos) + dir_to_target * config.guide_offset stagnant_start_time = None return True, guide_target.tolist() else: stagnant_start_time = None + return False, target_pos -def adaptive_potential_params(ee_pos, obstacle_list): - """自适应势场参数""" +def adaptive_potential_params(ee_pos, obstacle_list, config): obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list] min_dist = min(obs_distances) if obs_distances else 1.0 - k_rep = OBSTACLE_CONFIG["base_k_rep"] if min_dist > 0.2 else OBSTACLE_CONFIG["base_k_rep"] * 2.0 - k_att = OBSTACLE_CONFIG["base_k_att"] if len(obstacle_list) <= 2 else OBSTACLE_CONFIG["base_k_att"] * 0.5 + + k_rep = config.base_k_rep if min_dist > 0.2 else config.base_k_rep * 2.0 + k_att = config.base_k_att if len(obstacle_list) <= 2 else config.base_k_att * 0.5 return k_att, k_rep -def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list): - """鲁棒版人工势场法""" +def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list, config): ee_pos = np.array(ee_pos) target_pos = np.array(target_pos) # 局部最优规避 - is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos) + is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos, config) current_target = np.array(guide_target) if is_local_opt else target_pos # 自适应参数 - k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list) + k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list, config) # 引力+斥力计算 att_force = k_att * (current_target - ee_pos) rep_force = np.zeros(3) + for obs in obstacle_list: obs_pos = np.array(obs[:3]) obs_radius = obs[3] dist = np.linalg.norm(ee_pos - obs_pos) - if dist < OBSTACLE_CONFIG["rep_radius"] + obs_radius: + + if dist < config.rep_radius + obs_radius: rep_dir = (ee_pos - obs_pos) / (dist + 1e-6) - rep_force += k_rep * (1 / (dist - obs_radius) - 1 / OBSTACLE_CONFIG["rep_radius"]) * ( - 1 / dist ** 2) * rep_dir + rep_force += k_rep * (1 / (dist - obs_radius) - 1 / config.rep_radius) * (1 / dist ** 2) * rep_dir # 修正目标并约束 corrected_target = ee_pos + att_force + rep_force @@ -151,47 +402,25 @@ def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list) return corrected_target.tolist() -def collision_check_approx(ee_pos, joint_pos, obstacle_list): - """碰撞冗余检测""" - ee_collision = False - min_ee_dist = 100.0 - for obs in obstacle_list: - obs_pos = np.array(obs[:3]) - obs_radius = obs[3] - dist = np.linalg.norm(np.array(ee_pos) - obs_pos) - min_ee_dist = min(min_ee_dist, dist) - if dist < obs_radius + EFFICIENCY_CONFIG["safety_margin"]: - ee_collision = True - break - return ee_collision, min_ee_dist - - -# ====================== 4. 效率优化核心:时间最优轨迹规划 ====================== -def time_optimal_joint_trajectory(start_joint, end_joint, seg_time): - """ - 时间最优关节轨迹(梯形速度曲线,满足速度/加速度约束) - :return: 时间最优的关节位置/速度/加速度轨迹 - """ +# ====================== 7. 轨迹规划 ====================== +def time_optimal_joint_trajectory(start_joint, end_joint, seg_time, config): n_joints = 6 - traj_points = EFFICIENCY_CONFIG["traj_interp_points"] + traj_points = config.traj_interp_points t_steps = np.linspace(0, seg_time, traj_points) - # 初始化轨迹数组 opt_pos = np.zeros((traj_points, n_joints)) opt_vel = np.zeros((traj_points, n_joints)) opt_acc = np.zeros((traj_points, n_joints)) for j in range(n_joints): delta = end_joint[j] - start_joint[j] - max_vel = CONSTRAINTS["max_vel"][j] - max_acc = CONSTRAINTS["max_acc"][j] + max_vel = config.max_vel[j] + max_acc = config.max_acc[j] - # 计算梯形速度曲线的关键时间点 - t_acc = max_vel / max_acc # 加速时间 - s_acc = 0.5 * max_acc * t_acc ** 2 # 加速段位移 + t_acc = max_vel / max_acc + s_acc = 0.5 * max_acc * t_acc ** 2 if abs(delta) < 2 * s_acc: - # 三角形速度曲线(未到最大速度) t_joint = 2 * np.sqrt(abs(delta) / max_acc) for i, t in enumerate(t_steps): if t <= t_joint / 2: @@ -204,161 +433,128 @@ def time_optimal_joint_trajectory(start_joint, end_joint, seg_time): opt_vel[i, j] = max_acc * t_rem * np.sign(delta) opt_acc[i, j] = -max_acc * np.sign(delta) else: - # 梯形速度曲线(达到最大速度) - t_const = (abs(delta) - 2 * s_acc) / max_vel # 匀速时间 + t_const = (abs(delta) - 2 * s_acc) / max_vel t_joint = 2 * t_acc + t_const for i, t in enumerate(t_steps): if t <= t_acc: - # 加速段 opt_pos[i, j] = start_joint[j] + 0.5 * max_acc * t ** 2 * np.sign(delta) opt_vel[i, j] = max_acc * t * np.sign(delta) opt_acc[i, j] = max_acc * np.sign(delta) elif t <= t_acc + t_const: - # 匀速段 opt_pos[i, j] = start_joint[j] + (s_acc + max_vel * (t - t_acc)) * np.sign(delta) opt_vel[i, j] = max_vel * np.sign(delta) opt_acc[i, j] = 0.0 else: - # 减速段 t_rem = t_joint - t opt_pos[i, j] = end_joint[j] - 0.5 * max_acc * t_rem ** 2 * np.sign(delta) opt_vel[i, j] = max_acc * t_rem * np.sign(delta) opt_acc[i, j] = -max_acc * np.sign(delta) - # 约束速度/加速度 opt_vel[:, j] = np.clip(opt_vel[:, j], -max_vel, max_vel) opt_acc[:, j] = np.clip(opt_acc[:, j], -max_acc, max_acc) + # 平滑关节轨迹(使用修复后的函数) + opt_pos = smooth_joint_traj(opt_pos, config.smooth_factor) + return opt_pos, opt_vel, opt_acc -# ====================== 5. 效率优化核心:能耗最优二次规划(兼容所有求解器) ====================== -def energy_optimal_trajectory(joint_waypoints, seg_time): - """ - 能耗最优轨迹(二次规划求解,最小化扭矩平方积分) - :return: 能耗最优的关节位置轨迹 - """ +def energy_optimal_trajectory(joint_waypoints, seg_time, config): n_joints = 6 n_points = len(joint_waypoints) t_step = seg_time / (n_points - 1) - # 定义优化变量 - q = cp.Variable((n_joints, n_points)) # 关节位置 - qd = cp.Variable((n_joints, n_points)) # 关节速度 - qdd = cp.Variable((n_joints, n_points)) # 关节加速度 + q = cp.Variable((n_joints, n_points)) + qd = cp.Variable((n_joints, n_points)) + qdd = cp.Variable((n_joints, n_points)) - # 代价函数:最小化能耗(扭矩平方积分≈加速度平方积分) energy_cost = cp.sum_squares(qdd) - time_cost = cp.sum(cp.max(cp.abs(qd), axis=1)) # 时间代价:速度越大时间越短 - total_cost = EFFICIENCY_CONFIG["time_weight"] * time_cost + EFFICIENCY_CONFIG["energy_weight"] * energy_cost + time_cost = cp.sum(cp.max(cp.abs(qd), axis=1)) + total_cost = config.time_weight * time_cost + config.energy_weight * energy_cost - # 约束条件 constraints = [] - # 初始/终止条件 constraints.append(q[:, 0] == joint_waypoints[0]) constraints.append(q[:, -1] == joint_waypoints[-1]) constraints.append(qd[:, 0] == 0) constraints.append(qd[:, -1] == 0) - # 速度/加速度约束 + for j in range(n_joints): - constraints.append(qd[j, :] <= CONSTRAINTS["max_vel"][j]) - constraints.append(qd[j, :] >= -CONSTRAINTS["max_vel"][j]) - constraints.append(qdd[j, :] <= CONSTRAINTS["max_acc"][j]) - constraints.append(qdd[j, :] >= -CONSTRAINTS["max_acc"][j]) - # 动力学约束(差分) + constraints.append(qd[j, :] <= config.max_vel[j]) + constraints.append(qd[j, :] >= -config.max_vel[j]) + constraints.append(qdd[j, :] <= config.max_acc[j]) + constraints.append(qdd[j, :] >= -config.max_acc[j]) + for i in range(n_points - 1): constraints.append(qd[:, i + 1] == (q[:, i + 1] - q[:, i]) / t_step) constraints.append(qdd[:, i + 1] == (qd[:, i + 1] - qd[:, i]) / t_step) - # 求解二次规划(自动选择可用求解器,增加容错) prob = cp.Problem(cp.Minimize(total_cost), constraints) try: - # 优先尝试ECOS求解器 prob.solve(solver=cp.ECOS, verbose=False) except: try: - # 备选:OSQP求解器(CVXPY默认推荐) prob.solve(solver=cp.OSQP, verbose=False) except: - # 最后:使用CVXPY自动选择的求解器 prob.solve(verbose=False) if prob.status != cp.OPTIMAL: - print("⚠️ 能耗优化求解失败,降级为时间最优轨迹") + logger.warning("能耗优化求解失败,降级为时间最优轨迹") return None + return q.value.T -# ====================== 6. 效率+鲁棒融合:避障轨迹的双优优化(修复索引越界) ====================== -def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, obstacle_list): - """ - 融合避障鲁棒性+时间/能耗最优的轨迹规划 - :return: 优化后的关节目标、当前段能耗 - """ +def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, config): global total_motion_time, total_energy_consume - # 步骤1:鲁棒避障修正笛卡尔目标 + # 1. 鲁棒避障修正目标 ee_vel = get_ee_cartesian_velocity(model, data, mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")) - corrected_cart_target = robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list) + corrected_cart_target = robust_artificial_potential_field(ee_pos, ee_vel, target_pos, config.obstacle_list, config) - # 步骤2:逆解得到关节目标 + # 2. 平滑笛卡尔目标轨迹(使用修复后的函数) + corrected_cart_target = smooth_cartesian_traj([ee_pos, corrected_cart_target], config.smooth_factor)[-1] + + # 3. 逆解得到关节目标 ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") data.site_xpos[ee_site_id] = corrected_cart_target mujoco.mj_inverse(model, data) end_joint = data.qpos[:6].copy() - start_joint = data.qpos[:6].copy() # 当前关节位置为起点 + start_joint = data.qpos[:6].copy() - # 步骤3:时间最优轨迹初值 - seg_time = 2.0 # 初始段时间 - time_opt_pos, time_opt_vel, time_opt_acc = time_optimal_joint_trajectory(start_joint, end_joint, seg_time) + # 4. 时间最优轨迹 + seg_time = 2.0 + time_opt_pos, time_opt_vel, time_opt_acc = time_optimal_joint_trajectory(start_joint, end_joint, seg_time, config) - # 步骤4:能耗最优优化(二次规划) - energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time) + # 5. 能耗最优优化 + energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time, config) if energy_opt_pos is None: final_joint_traj = time_opt_pos else: final_joint_traj = energy_opt_pos - # 步骤5:计算当前段能耗(扭矩平方积分,修复索引越界错误) + # 6. 计算真实能耗 + dt = seg_time / len(final_joint_traj) seg_energy = 0.0 - # 遍历每个轨迹点 + for traj_idx in range(len(final_joint_traj)): - # 跳过第一个点(无加速度) if traj_idx == 0: continue - # 遍历每个关节计算扭矩 - for joint_idx in range(6): - # 获取当前轨迹点和上一个轨迹点的关节角度 - curr_angle = final_joint_traj[traj_idx, joint_idx] - prev_angle = final_joint_traj[traj_idx - 1, joint_idx] - - # 计算关节速度(差分) - dt = seg_time / len(final_joint_traj) - joint_vel = (curr_angle - prev_angle) / dt - - # 计算关节加速度(差分,使用前一个速度) - if traj_idx == 1: - joint_acc = joint_vel / dt - else: - prev_vel = (final_joint_traj[traj_idx - 1, joint_idx] - final_joint_traj[traj_idx - 2, joint_idx]) / dt - joint_acc = (joint_vel - prev_vel) / dt - - # 计算扭矩和能耗(积分) - torque = JOINT_INERTIA[joint_idx] * joint_acc + JOINT_GRAVITY[joint_idx] - seg_energy += np.square(torque) * dt + # 使用真实扭矩计算能耗 + seg_energy += calculate_real_energy_consumption(model, data, config, dt) # 更新全局统计 total_motion_time += seg_time total_energy_consume += seg_energy - # 返回当前时刻的关节目标(取第一个插值点) + # 记录轨迹历史 + traj_history.append(corrected_cart_target) + return final_joint_traj[0], corrected_cart_target, seg_energy -# ====================== 7. 机械臂模型(修复XML语法错误) ====================== -def get_arm_xml_with_obstacles(): - """生成带障碍的机械臂XML模型(修复inertial标签错误)""" +# ====================== 8. 机械臂模型 ====================== +def get_arm_xml_with_obstacles(config): arm_xml = """ @@ -372,37 +568,30 @@ def get_arm_xml_with_obstacles(): - - - - - - - @@ -414,15 +603,16 @@ def get_arm_xml_with_obstacles(): - """ - # 添加障碍 - for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]): + + # 添加障碍物 + for i, obs in enumerate(config.obstacle_list): x, y, z, r = obs material = "critical_obstacle" if i == 0 else "obstacle" arm_xml += f""" """ + arm_xml += """ @@ -438,11 +628,12 @@ def get_arm_xml_with_obstacles(): return arm_xml -# ====================== 8. 仿真主逻辑 ====================== -def run_efficiency_optimized_simulation(): - """运行效率+鲁棒双优化的仿真""" +# ====================== 9. 仿真主逻辑 ====================== +def run_enhanced_simulation(): global total_motion_time, total_energy_consume - arm_xml = get_arm_xml_with_obstacles() + + # 生成模型XML + arm_xml = get_arm_xml_with_obstacles(config) with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f: f.write(arm_xml) @@ -451,60 +642,66 @@ def run_efficiency_optimized_simulation(): try: model = mujoco.MjModel.from_xml_path(xml_path) data = mujoco.MjData(model) - print("✅ 效率+鲁棒双优化机械臂模型加载成功!") - print(f"🔧 效率配置:时间权重={EFFICIENCY_CONFIG['time_weight']}, 能耗权重={EFFICIENCY_CONFIG['energy_weight']}") - print(f"🔧 鲁棒配置:局部最优规避 + 自适应势场 + 碰撞冗余检测") + logger.info("✅ 增强版机械臂仿真模型加载成功!") + logger.info( + f"🔧 配置:轨迹点数={config.traj_interp_points}, 时间权重={config.time_weight}, 平滑系数={config.smooth_factor}") - # 预计算笛卡尔轨迹对应的关节起点 + # 预计算关节起点 ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") joint_waypoints = [] - for cart_pos in CART_WAYPOINTS: + + for cart_pos in config.cart_waypoints: mujoco.mj_resetData(model, data) data.site_xpos[ee_site_id] = cart_pos mujoco.mj_inverse(model, data) joint_waypoints.append(data.qpos[:6].copy()) + # 启动仿真视图 with mujoco.viewer.launch_passive(model, data) as viewer: - print("\n🎮 效率+鲁棒双优化仿真启动!") - print("💡 核心:避障安全 + 时间/能耗最优(工业级需求)") - print("💡 按 Ctrl+C 退出\n") + logger.info("\n🎮 增强版机械臂仿真启动!") + logger.info("💡 核心增强:全链路碰撞检测 + 轨迹平滑 + 真实能耗计算 + 可视化增强") + logger.info("💡 按 Ctrl+C 退出\n") - traj_idx = 0 current_waypoint = 0 last_print_time = 0.0 + pause_simulation = False while viewer.is_running(): + if pause_simulation: + time.sleep(0.1) + continue + t_total = data.time ee_pos = data.site_xpos[ee_site_id].tolist() - # 切换笛卡尔目标点 - if current_waypoint < len(CART_WAYPOINTS): - target_cart = CART_WAYPOINTS[current_waypoint] - # 距离目标点<0.01m时切换下一个 + # 切换目标点 + if current_waypoint < len(config.cart_waypoints): + target_cart = config.cart_waypoints[current_waypoint] if np.linalg.norm(np.array(ee_pos) - np.array(target_cart)) < 0.01: - current_waypoint = (current_waypoint + 1) % len(CART_WAYPOINTS) - print(f"\n🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}") + current_waypoint = (current_waypoint + 1) % len(config.cart_waypoints) + logger.info(f"\n🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}") else: - target_cart = CART_WAYPOINTS[-1] + target_cart = config.cart_waypoints[-1] - # 融合避障+效率优化的轨迹规划 + # 轨迹优化 target_joints, corrected_cart, seg_energy = optimize_obstacle_traj_with_efficiency( - model, data, ee_pos, target_cart, OBSTACLE_CONFIG["obstacle_list"] + model, data, ee_pos, target_cart, config ) - # 碰撞检测与紧急避障 - is_collision, min_obs_dist = collision_check_approx(ee_pos, target_joints, - OBSTACLE_CONFIG["obstacle_list"]) + # 全链路碰撞检测 + is_collision, min_obs_dist = full_arm_collision_check(model, data, config) + + # 紧急避障 if is_collision: - emergency_rep = np.array(ee_pos) - np.array(OBSTACLE_CONFIG["obstacle_list"][0][:3]) + logger.warning("🆘 检测到碰撞风险,执行紧急避障!") + emergency_rep = np.array(ee_pos) - np.array(config.obstacle_list[0][:3]) emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05 corrected_cart = np.array(corrected_cart) + emergency_rep data.site_xpos[ee_site_id] = corrected_cart mujoco.mj_inverse(model, data) target_joints = data.qpos[:6].copy() - print(f"🆘 紧急避障:修正目标={np.round(corrected_cart, 3)}") - # 闭环PD控制(带扭矩约束) + # PD控制 ctrl_signals = [] for i in range(6): k_p = 8.0 @@ -514,47 +711,54 @@ def run_efficiency_optimized_simulation(): pos_error = target_joints[i] - current_pos vel_error = -current_vel ctrl = k_p * pos_error + k_d * vel_error - # 扭矩约束(转换为控制量约束) - max_ctrl = CONSTRAINTS["max_torque"][i] / 100.0 # gear=100,直接计算 + max_ctrl = config.max_torque[i] / 100.0 ctrl = np.clip(ctrl, -max_ctrl, max_ctrl) ctrl_signals.append(ctrl) + data.ctrl[:6] = ctrl_signals - # 打印效率统计(每2秒) + # 打印统计信息 if t_total - last_print_time > 2.0 and t_total > 0: ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id) avg_vel = np.linalg.norm(ee_vel) avg_energy = total_energy_consume / t_total if t_total > 0 else 0.0 - print(f"\n⏱️ 时间:{t_total:.2f}s | 累计运动时间:{total_motion_time:.2f}s") - print(f" 末端位置:{np.round(ee_pos, 3)} | 目标位置:{np.round(corrected_cart, 3)}") - print(f" 末端速度:{avg_vel:.4f}m/s | 最近障碍:{min_obs_dist:.3f}m") - print(f" 累计能耗:{total_energy_consume:.2f}J | 平均能耗:{avg_energy:.2f}J/s") - print(f" 碰撞风险:{'是' if is_collision else '否'}") + + logger.info(f"\n⏱️ 仿真时间:{t_total:.2f}s | 累计运动时间:{total_motion_time:.2f}s") + logger.info(f" 末端位置:{np.round(ee_pos, 3)} | 目标位置:{np.round(corrected_cart, 3)}") + logger.info(f" 末端速度:{avg_vel:.4f}m/s | 最近障碍距离:{min_obs_dist:.3f}m") + logger.info(f" 累计能耗:{total_energy_consume:.2f}J | 平均能耗:{avg_energy:.2f}J/s") + logger.info(f" 碰撞风险:{'⚠️ 高' if is_collision else '✅ 低'}") last_print_time = t_total - # 仿真步运行 + # 增强可视化(适配新版API) + draw_enhanced_visualization(viewer, model, data, config) + + # 运行仿真步 mujoco.mj_step(model, data) viewer.sync() + try: mujoco.utils.mju_sleep(1 / 60) except: time.sleep(1 / 60) + except KeyboardInterrupt: + logger.info("\n🛑 用户终止仿真") except Exception as e: - print(f"❌ 仿真出错:{e}") + logger.error(f"❌ 仿真出错:{e}") import traceback traceback.print_exc() finally: os.unlink(xml_path) - # 打印最终效率统计 - print(f"\n📊 仿真结束 - 效率统计") - print(f" 总运动时间:{total_motion_time:.2f}s") - print(f" 总能耗:{total_energy_consume:.2f}J") - print( - f" 时间/能耗综合得分:{total_motion_time * EFFICIENCY_CONFIG['time_weight'] + total_energy_consume * EFFICIENCY_CONFIG['energy_weight']:.2f}") + # 最终统计 + logger.info(f"\n📊 仿真结束 - 最终统计") + logger.info(f" 总运动时间:{total_motion_time:.2f}s") + logger.info(f" 总能耗:{total_energy_consume:.2f}J") + logger.info( + f" 综合得分:{total_motion_time * config.time_weight + total_energy_consume * config.energy_weight:.2f}") if __name__ == "__main__": - # 安装依赖(首次运行需执行) - # pip install cvxpy scipy ecos osqp - run_efficiency_optimized_simulation() \ No newline at end of file + # 安装依赖 + # pip install cvxpy scipy ecos osqp mujoco + run_enhanced_simulation() \ No newline at end of file