diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py index 02ecbd8176..3c13f17028 100644 --- a/src/Robotic_arm/main.py +++ b/src/Robotic_arm/main.py @@ -5,21 +5,22 @@ import tempfile import time from scipy import interpolate -from sklearn.cluster import DBSCAN +import cvxpy as cp # 用于能耗最优的二次规划 import warnings warnings.filterwarnings("ignore") -# ====================== 1. 全局配置（鲁棒性优化参数） ====================== -# 物理约束（UR5参考） +# ====================== 1. 全局配置（鲁棒+效率双优化） ====================== +# 物理约束（UR5工业级参数） CONSTRAINTS = { - "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_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, # 基础斥力系数 @@ -34,246 +35,332 @@ ] } -# 笛卡尔轨迹关键点（易触发局部最优的路径） +# 效率优化参数（工业场景可配置） +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.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 # 累计能耗 +# 预定义关节惯性参数（适配所有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. 新增：兼容所有版本的末端速度计算 ====================== -def get_ee_cartesian_velocity(model, data, ee_site_id): - """ - 计算末端执行器的笛卡尔速度（兼容所有MuJoCo版本） - 原理：通过雅可比矩阵将关节速度转换为末端笛卡尔速度 - """ - # 获取雅可比矩阵（6xN，前3行是平移速度，后3行是旋转速度） - jacp = np.zeros((3, model.nv)) # 平移雅可比 - jacr = np.zeros((3, model.nv)) # 旋转雅可比 - # 计算末端site的雅可比矩阵 +# ====================== 2. 基础工具函数（兼容所有MuJoCo版本） ====================== +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) - - # 关节速度（data.qvel） - joint_vel = data.qvel[:6] # 仅取前6个关节速度 - - # 笛卡尔平移速度 = 雅可比 × 关节速度 + joint_vel = data.qvel[:6] ee_cart_vel = jacp @ joint_vel - return ee_cart_vel -# ====================== 3. 物理约束轨迹生成（原有逻辑） ====================== -def constrained_quintic_polynomial(start, end, total_time, t, joint_idx): - s0, v0, a0 = start, 0, 0 - s1, v1, a1 = end, 0, 0 - - T = total_time - a = s0 - b = v0 - c = a0 / 2 - d = (20 * (s1 - s0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T ** 2) / (2 * T ** 3) - e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4) - f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5) - - pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5 - vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4 - acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 4 - - vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx]) - acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx]) +def calculate_joint_torque(model, data, joint_idx): + """计算关节扭矩（能耗核心指标，适配新版MuJoCo）""" + # 方案1：使用预定义惯性参数（兼容所有版本） + inertia = JOINT_INERTIA[joint_idx] + gravity_comp = JOINT_GRAVITY[joint_idx] - return pos, vel, acc + # 方案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 -# ====================== 4. 闭环PD控制（原有逻辑） ====================== -def closed_loop_constraint_control(data, target_joints, joint_idx): - k_p = 8.0 - k_d = 0.2 - current_pos = data.qpos[joint_idx] - current_vel = data.qvel[joint_idx] - - pos_error = target_joints[joint_idx] - current_pos - vel_error = -current_vel - - ctrl = k_p * pos_error + k_d * vel_error - ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1]) - - return ctrl - - -# ====================== 5. 鲁棒性优化1：局部最优检测与规避 ====================== +# ====================== 3. 鲁棒性避障（保留原有核心逻辑） ====================== def check_local_optimum(ee_vel, ee_pos, target_pos): - """ - 检测是否陷入局部最优，并生成引导目标跳出陷阱 - :return: is_local_opt (是否局部最优), guide_target (引导目标位置) - """ + """检测局部最优并生成引导目标""" global stagnant_start_time - - # 计算末端合速度 vel_mag = np.linalg.norm(ee_vel) - threshold = OBSTACLE_CONFIG["stagnant_threshold"] - max_stagnant_time = OBSTACLE_CONFIG["stagnant_time"] - - if vel_mag < threshold: + if vel_mag < OBSTACLE_CONFIG["stagnant_threshold"]: if stagnant_start_time is None: stagnant_start_time = time.time() - # 超过停滞时间，判定为局部最优 - elif time.time() - stagnant_start_time > max_stagnant_time: - print(f"\n⚠️ 检测到局部最优！末端速度={vel_mag:.4f}m/s < 阈值={threshold}m/s") - # 生成引导目标：向原始目标方向偏移 + elif time.time() - stagnant_start_time > OBSTACLE_CONFIG["stagnant_time"]: + print(f"\n⚠️ 检测到局部最优！末端速度={vel_mag:.4f}m/s") dir_to_target = np.array(target_pos) - np.array(ee_pos) - if np.linalg.norm(dir_to_target) < 1e-6: - dir_to_target = np.array([0.0, 0.0, 0.1]) # 避免除零 - else: - dir_to_target = dir_to_target / np.linalg.norm(dir_to_target) - + 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"] - print(f"📌 生成引导目标：{np.round(guide_target, 3)} (偏移{OBSTACLE_CONFIG['guide_offset']}m)") - stagnant_start_time = None # 重置计时器 + stagnant_start_time = None return True, guide_target.tolist() else: - stagnant_start_time = None # 速度正常，重置计时器 - + stagnant_start_time = None return False, target_pos -# ====================== 6. 鲁棒性优化2：自适应势场参数 ====================== def adaptive_potential_params(ee_pos, obstacle_list): - """ - 根据障碍距离/数量自适应调整引力/斥力系数 - - 距离越近，斥力越大；障碍越多，引力越小 - """ - base_k_att = OBSTACLE_CONFIG["base_k_att"] - base_k_rep = OBSTACLE_CONFIG["base_k_rep"] - - # 计算与最近障碍的距离 + """自适应势场参数""" 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 - obs_count = len(obstacle_list) - - # 距离自适应斥力系数：距离<0.2m时，斥力翻倍 - k_rep = base_k_rep if min_dist > 0.2 else base_k_rep * 2.0 - # 数量自适应引力系数：障碍>2个时，引力降低50% - k_att = base_k_att if obs_count <= 2 else base_k_att * 0.5 - + 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 return k_att, k_rep -# ====================== 7. 鲁棒性优化3：碰撞冗余检测 ====================== -def collision_check_approx(ee_pos, joint_pos, obstacle_list, safety_margin=0.05): - """ - 近似碰撞检测（工程简化版）：检测末端+关键关节与障碍的距离 - :return: is_collision (是否碰撞), min_safe_dist (最小安全距离) - """ - # 检测末端执行器 - 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 + safety_margin: - ee_collision = True - break - - # 检测关键关节（简化：仅检测关节2/3/4） - joint_collision = False - # 仿真中通过data获取关节位置（实际场景需正运动学计算） - # 这里简化为基于关节角度的近似检测 - joint_2_3_4_idx = [2, 3, 4] - for idx in joint_2_3_4_idx: - # 近似关节位置（基于机械臂模型） - joint_pos_approx = np.array([ - 0.4 + 0.35 * np.cos(joint_pos[2]), - 0.0 + 0.35 * np.sin(joint_pos[2]), - 0.5 + 0.25 * np.sin(joint_pos[3]) - ]) - for obs in obstacle_list: - obs_pos = np.array(obs[:3]) - obs_radius = obs[3] - dist = np.linalg.norm(joint_pos_approx - obs_pos) - if dist < obs_radius + safety_margin: - joint_collision = True - break - if joint_collision: - break - - is_collision = ee_collision or joint_collision - if is_collision: - print(f"\n🚨 碰撞风险！末端与最近障碍距离={min_ee_dist:.3f}m < 安全裕度={safety_margin}m") - - return is_collision, min_ee_dist - - -# ====================== 8. 鲁棒性优化后的避障核心逻辑 ====================== def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list): - """ - 鲁棒版人工势场法：局部最优规避 + 自适应参数 - """ + """鲁棒版人工势场法""" ee_pos = np.array(ee_pos) target_pos = np.array(target_pos) - rep_radius = OBSTACLE_CONFIG["rep_radius"] - # 步骤1：检测局部最优，生成引导目标 + # 局部最优规避 is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos) current_target = np.array(guide_target) if is_local_opt else target_pos - # 步骤2：自适应调整引力/斥力系数 + # 自适应参数 k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list) - print( - f"\n🔧 自适应参数：k_att={k_att:.1f}, k_rep={k_rep:.1f} (最近障碍距离={min([np.linalg.norm(ee_pos - np.array(obs[:3])) for obs in obstacle_list]):.3f}m)") - # 步骤3：计算引力（指向当前目标） + # 引力+斥力计算 att_force = k_att * (current_target - ee_pos) - - # 步骤4：计算斥力（远离所有障碍） 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: + 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 - if dist < rep_radius + obs_radius: - if dist < 1e-6: - dist = 1e-6 - rep_dir = (ee_pos - obs_pos) / dist - # 优化斥力公式：避免距离过近时斥力突变 - rep_force += k_rep * (1 / (dist - obs_radius) - 1 / rep_radius) * (1 / dist ** 2) * rep_dir - - # 步骤5：合力修正目标位置，添加边界约束 + # 修正目标并约束 corrected_target = ee_pos + att_force + rep_force corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0]) return corrected_target.tolist() -# ====================== 9. 逆运动学预计算（兼容旧版MuJoCo） ====================== -def precompute_joint_waypoints(model, data, cart_waypoints): - joint_waypoints = [] +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: 时间最优的关节位置/速度/加速度轨迹 + """ + n_joints = 6 + traj_points = EFFICIENCY_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] + + # 计算梯形速度曲线的关键时间点 + 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: + 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) + 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) + else: + # 梯形速度曲线（达到最大速度） + 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) + + return opt_pos, opt_vel, opt_acc + + +# ====================== 5. 效率优化核心：能耗最优二次规划（兼容所有求解器） ====================== +def energy_optimal_trajectory(joint_waypoints, seg_time): + """ + 能耗最优轨迹（二次规划求解，最小化扭矩平方积分） + :return: 能耗最优的关节位置轨迹 + """ + 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)) # 关节加速度 + + # 代价函数：最小化能耗（扭矩平方积分≈加速度平方积分） + 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 + + # 约束条件 + 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]) + # 动力学约束（差分） + 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("⚠️ 能耗优化求解失败，降级为时间最优轨迹") + return None + return q.value.T + + +# ====================== 6. 效率+鲁棒融合：避障轨迹的双优优化（修复索引越界） ====================== +def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, obstacle_list): + """ + 融合避障鲁棒性+时间/能耗最优的轨迹规划 + :return: 优化后的关节目标、当前段能耗 + """ + global total_motion_time, total_energy_consume + + # 步骤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) + + # 步骤2：逆解得到关节目标 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() # 当前关节位置为起点 + + # 步骤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：能耗最优优化（二次规划） + energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time) + if energy_opt_pos is None: + final_joint_traj = time_opt_pos + else: + final_joint_traj = energy_opt_pos + + # 步骤5：计算当前段能耗（扭矩平方积分，修复索引越界错误） + 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 - for cart_pos in 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()) + # 更新全局统计 + total_motion_time += seg_time + total_energy_consume += seg_energy - return joint_waypoints + # 返回当前时刻的关节目标（取第一个插值点） + return final_joint_traj[0], corrected_cart_target, seg_energy -# ====================== 10. 机械臂模型（带密集障碍可视化） ====================== +# ====================== 7. 机械臂模型（修复XML语法错误） ====================== def get_arm_xml_with_obstacles(): + """生成带障碍的机械臂XML模型（修复inertial标签错误）""" arm_xml = """ - +