diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py index 71db2161ac..3ea2964733 100644 --- a/src/Robotic_arm/main.py +++ b/src/Robotic_arm/main.py @@ -6,33 +6,39 @@ import time from scipy import interpolate -# ====================== 1. 定义机械臂物理约束参数(核心) ====================== -# 参考UR5机械臂参数,可根据实际硬件调整 +# ====================== 1. 全局配置(物理约束+避障参数) ====================== 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³) - "ctrl_limit": [-10.0, 10.0] # 电机控制量限制 + "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], + "ctrl_limit": [-10.0, 10.0] } +OBSTACLE_CONFIG = { + "k_att": 0.8, + "k_rep": 0.6, + "rep_radius": 0.3, + "obstacle_list": [ + [0.6, 0.1, 0.5, 0.1], + [0.4, -0.1, 0.6, 0.08] + ] +} + +CART_WAYPOINTS = [ + [0.5, 0.0, 0.6], + [0.7, 0.2, 0.7], + [0.8, 0.1, 0.8], + [0.7, 0.2, 0.7], + [0.5, 0.0, 0.6] +] + -# ====================== 2. 带约束的五次多项式轨迹生成 ====================== +# ====================== 2. 物理约束轨迹生成 ====================== def constrained_quintic_polynomial(start, end, total_time, t, joint_idx): - """ - 带约束的五次多项式插值 - :param start: 起点角度 - :param end: 终点角度 - :param total_time: 轨迹段总时间 - :param t: 当前段内时间 (0<=t<=total_time) - :param joint_idx: 关节索引(0-5) - :return: 约束后的位置、速度、加速度 - """ - # 基础边界条件(启停时速度/加速度为0) s0, v0, a0 = start, 0, 0 s1, v1, a1 = end, 0, 0 T = total_time - # 五次多项式系数计算 a = s0 b = v0 c = a0 / 2 @@ -40,61 +46,111 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx): 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 ** 3 - jerk = 6 * d + 24 * e * t + 60 * f * t ** 2 - # 应用约束(核心:超出则截断) - max_vel = CONSTRAINTS["max_vel"][joint_idx] - max_acc = CONSTRAINTS["max_acc"][joint_idx] - max_jerk = CONSTRAINTS["max_jerk"][joint_idx] + 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]) - vel = np.clip(vel, -max_vel, max_vel) - acc = np.clip(acc, -max_acc, max_acc) - jerk = np.clip(jerk, -max_jerk, max_jerk) - - # 可选:如果速度/加速度被截断,反向修正位置(更严谨) - # 这里简化处理,直接返回约束后的位置(基础场景足够) return pos, vel, acc -# ====================== 3. 闭环约束控制(实时修正) ====================== +# ====================== 3. 闭环PD控制 ====================== def closed_loop_constraint_control(data, target_joints, joint_idx): - """ - 闭环PD控制 + 约束检查,实时修正控制指令 - """ - # PD控制参数(可根据实际机械臂标定) - k_p = 8.0 # 比例系数 - k_d = 0.2 # 微分系数 + 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 # 速度误差:目标速度为0(启停阶段) + 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 -# ====================== 4. 机械臂模型(不变) ====================== -arm_xml = """ - +# ====================== 4. 人工势场法避障 ====================== +def artificial_potential_field(ee_pos, target_pos): + ee_pos = np.array(ee_pos) + target_pos = np.array(target_pos) + obstacle_list = OBSTACLE_CONFIG["obstacle_list"] + k_att = OBSTACLE_CONFIG["k_att"] + k_rep = OBSTACLE_CONFIG["k_rep"] + rep_radius = OBSTACLE_CONFIG["rep_radius"] + + # 引力 + att_force = k_att * (target_pos - 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 < 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 + + # 修正目标位置 + 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() + + +# ====================== 5. 笛卡尔轨迹平滑 ====================== +def smooth_cartesian_trajectory(waypoints, num_points=200): + x = np.array([p[0] for p in waypoints]) + y = np.array([p[1] for p in waypoints]) + z = np.array([p[2] for p in waypoints]) + + t = np.linspace(0, 1, len(x)) + t_new = np.linspace(0, 1, num_points) + + spline_x = interpolate.CubicSpline(t, x, bc_type='natural') + spline_y = interpolate.CubicSpline(t, y, bc_type='natural') + spline_z = interpolate.CubicSpline(t, z, bc_type='natural') + + return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T + + +# ====================== 6. 替代逆运动学:关节轨迹预生成 ====================== +def precompute_joint_waypoints(model, data, cart_waypoints): + """预计算笛卡尔轨迹对应的关节角度(兼容旧版MuJoCo)""" + joint_waypoints = [] + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + + for cart_pos in cart_waypoints: + # 重置数据 + mujoco.mj_resetData(model, data) + # 设置目标位置到data中(旧版mj_inverse的要求) + data.site_xpos[ee_site_id] = cart_pos + # 调用逆运动学(仅model和data参数) + mujoco.mj_inverse(model, data) + # 保存关节角度 + joint_waypoints.append(data.qpos[:6].copy()) + + return joint_waypoints + + +# ====================== 7. 机械臂模型(带障碍物) ====================== +def get_arm_xml_with_obstacles(): + arm_xml = """ + -""" + """ + return arm_xml -# ====================== 5. 带约束的仿真主逻辑 ====================== -def run_constrained_simulation(): - # 临时XML文件 +# ====================== 8. 主仿真逻辑(修复逆运动学调用) ====================== +def run_obstacle_avoidance_simulation(): + arm_xml = get_arm_xml_with_obstacles() with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f: f.write(arm_xml) xml_path = f.name try: - # 加载模型和数据 model = mujoco.MjModel.from_xml_path(xml_path) data = mujoco.MjData(model) - print("✅ 带约束的机械臂模型加载成功!") - print("🔧 约束参数:") - print(f" 最大关节速度:{CONSTRAINTS['max_vel']} rad/s") - print(f" 最大关节加速度:{CONSTRAINTS['max_acc']} rad/s²") - - # 轨迹关键点(关节空间) - waypoints = [ - [0, 0.2, -0.5, 0, 0.3, 0], - [0.5, 0.5, -0.8, 0.2, 0.5, 0.3], - [0.8, 0.3, -0.6, 0.4, 0.2, 0.5], - [0.5, 0.5, -0.8, 0.2, 0.5, 0.3], - [0, 0.2, -0.5, 0, 0.3, 0] - ] - segment_time = 3.0 # 每段轨迹时长 - - # 启动可视化 + print("✅ 带避障的机械臂模型加载成功!") + print( + f"🔧 避障参数:斥力半径={OBSTACLE_CONFIG['rep_radius']}m,障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}") + + # 预计算关节轨迹关键点(兼容旧版MuJoCo) + joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS) + # 平滑关节轨迹 + num_joint_points = 200 + smooth_joint_traj = [] + for joint_idx in range(6): + joint_vals = [wp[joint_idx] for wp in joint_waypoints] + t = np.linspace(0, 1, len(joint_vals)) + t_new = np.linspace(0, 1, num_joint_points) + spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural') + smooth_joint_traj.append(spline(t_new)) + # 转置为 [轨迹点, 关节] + smooth_joint_traj = np.array(smooth_joint_traj).T + + traj_length = len(smooth_joint_traj) + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + segment_time = 5.0 + with mujoco.viewer.launch_passive(model, data) as viewer: - print("\n🎮 带约束的机械臂仿真启动!") - print("💡 特征:速度/加速度/控制量约束 + 闭环PD控制") + print("\n🎮 机械臂避障仿真启动!") + print("💡 核心功能:人工势场法避障 + 物理约束 + PD闭环控制") + print("💡 可视化:红色半透明球体为障碍物,机械臂自动绕开") print("💡 按 Ctrl+C 退出") while viewer.is_running(): - # 1. 计算当前轨迹段 + # 1. 计算当前轨迹索引 t_total = data.time - seg_idx = int(t_total // segment_time) % (len(waypoints) - 1) - t_seg = t_total % segment_time - - # 2. 生成带约束的目标关节角度 - target_joints = [] - joint_vels = [] # 记录约束后的速度(用于调试) - for i in range(6): - pos, vel, acc = constrained_quintic_polynomial( - waypoints[seg_idx][i], - waypoints[seg_idx + 1][i], - segment_time, - t_seg, - i - ) - # 额外约束关节角度在可控范围 - pos = np.clip(pos, model.actuator_ctrlrange[i][0], model.actuator_ctrlrange[i][1]) - target_joints.append(pos) - joint_vels.append(vel) - - # 3. 闭环约束控制:修正控制指令 + traj_idx = int((t_total / segment_time) * traj_length) % traj_length + + # 2. 获取末端当前位置 + ee_pos = data.site_xpos[ee_site_id].tolist() + + # 3. 原始关节目标 + raw_joint_target = smooth_joint_traj[traj_idx] + + # 4. 笛卡尔空间避障修正(核心) + # 先通过正运动学获取原始笛卡尔目标 + mujoco.mj_forward(model, data) # 更新正运动学 + raw_cart_target = data.site_xpos[ee_site_id].copy() + # 避障修正笛卡尔目标 + corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target) + # 修正后的笛卡尔目标转关节目标(兼容旧版) + data.site_xpos[ee_site_id] = corrected_cart_target + mujoco.mj_inverse(model, data) + target_joints = data.qpos[:6].copy() + + # 5. 应用物理约束 + 闭环控制 ctrl_signals = [] for i in range(6): + # 约束关节角度 + target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0], + model.actuator_ctrlrange[i][1]) + # 闭环PD控制 ctrl = closed_loop_constraint_control(data, target_joints, i) ctrl_signals.append(ctrl) - # 4. 应用控制指令 + # 6. 发送控制指令 data.ctrl[:6] = ctrl_signals - # 5. 打印关键状态(每50步打印一次,方便调试) - if int(data.time * 100) % 50 == 0: - print(f"\n⏱️ 时间:{data.time:.2f}s") - print(f" 关节0当前速度:{data.qvel[0]:.3f} rad/s (约束上限:{CONSTRAINTS['max_vel'][0]})") - print(f" 关节0控制量:{ctrl_signals[0]:.3f} (约束范围:{CONSTRAINTS['ctrl_limit']})") + # 7. 打印状态 + if int(t_total) % 1 == 0 and int(t_total) != 0: + obs_distances = [] + for obs in OBSTACLE_CONFIG["obstacle_list"]: + dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) + obs_distances.append(dist) + min_obs_dist = min(obs_distances) if obs_distances else 0 + + print(f"\n⏱️ 时间:{t_total:.2f}s") + print(f" 末端当前位置:{np.round(ee_pos, 3)}") + print(f" 修正后目标位置:{np.round(corrected_cart_target, 3)}") + print(f" 与最近障碍距离:{min_obs_dist:.3f}m (斥力半径:{OBSTACLE_CONFIG['rep_radius']}m)") - # 6. 运行仿真步 + # 8. 运行仿真步 mujoco.mj_step(model, data) viewer.sync() - # 7. 帧率控制 + # 9. 帧率控制 try: mujoco.utils.mju_sleep(1 / 60) except: @@ -219,9 +303,11 @@ def run_constrained_simulation(): except Exception as e: print(f"❌ 仿真出错:{e}") + import traceback + traceback.print_exc() finally: os.unlink(xml_path) if __name__ == "__main__": - run_constrained_simulation() \ No newline at end of file + run_obstacle_avoidance_simulation() \ No newline at end of file