diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py index b7ee4eee6a..71db2161ac 100644 --- a/src/Robotic_arm/main.py +++ b/src/Robotic_arm/main.py @@ -6,13 +6,33 @@ import time from scipy import interpolate - -# ====================== 轨迹优化核心(纯原生) ====================== -def quintic_polynomial(start, end, T, t): - """五次多项式插值:保证位置/速度/加速度连续""" +# ====================== 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³) + "ctrl_limit": [-10.0, 10.0] # 电机控制量限制 +} + + +# ====================== 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 @@ -20,10 +40,53 @@ def quintic_polynomial(start, end, T, t): 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) - return a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * 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, -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. 闭环约束控制(实时修正) ====================== +def closed_loop_constraint_control(data, target_joints, joint_idx): + """ + 闭环PD控制 + 约束检查,实时修正控制指令 + """ + # PD控制参数(可根据实际机械臂标定) + 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(启停阶段) + # 计算原始控制量 + 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 = """ @@ -75,18 +138,21 @@ def quintic_polynomial(start, end, T, t): """ -# ====================== 仿真运行 ====================== -def run_simulation(): +# ====================== 5. 带约束的仿真主逻辑 ====================== +def run_constrained_simulation(): # 临时XML文件 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("🔧 约束参数:") + print(f" 最大关节速度:{CONSTRAINTS['max_vel']} rad/s") + print(f" 最大关节加速度:{CONSTRAINTS['max_acc']} rad/s²") # 轨迹关键点(关节空间) waypoints = [ @@ -100,42 +166,62 @@ def run_simulation(): # 启动可视化 with mujoco.viewer.launch_passive(model, data) as viewer: - print("🎮 仿真启动!机械臂沿平滑轨迹运动(按Ctrl+C退出)") + print("\n🎮 带约束的机械臂仿真启动!") + print("💡 特征:速度/加速度/控制量约束 + 闭环PD控制") + print("💡 按 Ctrl+C 退出") + while viewer.is_running(): - # 计算当前轨迹段 + # 1. 计算当前轨迹段 t_total = data.time seg_idx = int(t_total // segment_time) % (len(waypoints) - 1) t_seg = t_total % segment_time - # 生成平滑轨迹 - target = [] + # 2. 生成带约束的目标关节角度 + target_joints = [] + joint_vels = [] # 记录约束后的速度(用于调试) for i in range(6): - pos = quintic_polynomial( + pos, vel, acc = constrained_quintic_polynomial( waypoints[seg_idx][i], waypoints[seg_idx + 1][i], segment_time, - t_seg + t_seg, + i ) - # 限制关节范围 + # 额外约束关节角度在可控范围 pos = np.clip(pos, model.actuator_ctrlrange[i][0], model.actuator_ctrlrange[i][1]) - target.append(pos) + target_joints.append(pos) + joint_vels.append(vel) + + # 3. 闭环约束控制:修正控制指令 + ctrl_signals = [] + for i in range(6): + ctrl = closed_loop_constraint_control(data, target_joints, i) + ctrl_signals.append(ctrl) + + # 4. 应用控制指令 + 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']})") - # 控制机械臂 - data.ctrl[:6] = target + # 6. 运行仿真步 mujoco.mj_step(model, data) viewer.sync() - # 帧率控制 + # 7. 帧率控制 try: mujoco.utils.mju_sleep(1 / 60) except: time.sleep(1 / 60) except Exception as e: - print(f"❌ 错误:{e}") + print(f"❌ 仿真出错:{e}") finally: os.unlink(xml_path) if __name__ == "__main__": - run_simulation() # 修复:删掉了多余的 + 号 \ No newline at end of file + run_constrained_simulation() \ No newline at end of file