robot_arm/arm_main.py

import numpy as np
from scipy.optimize import minimize
import serial
import time
import math
import matplotlib.pyplot as plt

class RobotArmUltimate:
    def __init__(self, port='COM3', baud=115200):
        # --- 1. 物理参数 ---
        self.L1, self.L2, self.L3, self.L4 = 70.0, 75.0, 50.0, 130.0
        self.current_servos_logic = np.array([90.0, 90.0, 90.0, 90.0])
        self.last_sent_servos = np.array([0.0, 0.0, 0.0, 0.0]) # 记录上次发送的值
        self.path_history = []
        
        # --- 【新增】倾斜修正参数 ---
        # 如果水平移动时往下掉，增加 OFFSET_Y (例如 2.0)
        self.OFFSET_Y = 0.0  
        self.OFFSET_Z = 0.0  

        # --- 2. 减震参数 ---
        self.servo_buffer = []  # 指令缓冲用于平滑滤波
        self.buffer_size = 3    # 移动平均窗口大小
        self.max_servo_speed = 30.0  # 舵机最大速度
        self.damping_factor = 0.7  # 阻尼因子

        try:
            self.ser = serial.Serial(port, baud, timeout=1)
            time.sleep(4) 
            self.ser.flushInput()
            print(">>> [系统就绪] 已加载 S-Curve 平滑减震算法 + 倾斜修正。")
            self.gripper_control(70)
        except:
            print(">>> [警告] 串口未连接，进入仿真。")
            self.ser = None

    def dh_matrix(self, theta_deg, d, a, alpha_deg):
        theta, alpha = np.radians(theta_deg), np.radians(alpha_deg)
        return np.array([
            [np.cos(theta), -np.sin(theta)*np.cos(alpha),  np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
            [np.sin(theta),  np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
            [0,              np.sin(alpha),               np.cos(alpha),              d],
            [0,              0,                           0,                          1]
        ])

    def forward_kinematics(self, s1, s2, s3, s4):
        t1, t2, t3, t4 = s1-90, 180-s2, 90-s3, s4-90
        T01 = self.dh_matrix(t1, self.L1, 0, 90)
        T12 = T01 @ self.dh_matrix(t2, 0, self.L2, 0)
        T23 = T12 @ self.dh_matrix(t3, 0, self.L3, 0)
        T34 = T23 @ self.dh_matrix(t4, 0, self.L4, 0)
        return T34[0:3, 3], t2 + t3 + t4, [np.array([0,0,0]), T01[0:3,3], T12[0:3,3], T23[0:3,3], T34[0:3,3]]

    def inverse_kinematics(self, target_xyz, target_pitch=-90, init_angles=None):
        if init_angles is None: init_angles = self.current_servos_logic
        def objective(s):
            pos, pitch, _ = self.forward_kinematics(*s)
            dist_err = np.linalg.norm(pos - target_xyz)
            pitch_err = abs(pitch - target_pitch)
            return dist_err * 2.0 + pitch_err * 0.1
        bounds = [(0, 180)] * 4
        res = minimize(objective, init_angles, bounds=bounds, method='SLSQP', tol=1e-3)
        return res.x

    def _send_and_audit(self, s, target_xyz):
        """带有多层减震的发送函数"""
        # --- 【修改点】应用倾斜修正 ---
        # 在进入滤波前，先加上固定的偏移量
        s_corrected = s.copy()
        s_corrected[1] += self.OFFSET_Y
        s_corrected[2] += self.OFFSET_Z

        s_logic = np.array([np.clip(x, 0, 180) for x in s_corrected])
        
        # --- 层1: 移动平均滤波器 ---
        self.servo_buffer.append(s_logic)
        if len(self.servo_buffer) > self.buffer_size:
            self.servo_buffer.pop(0)
        s_smoothed = np.mean(self.servo_buffer, axis=0)
        
        # --- 层2: 速度限制 ---
        delta = s_smoothed - self.current_servos_logic
        delta_norm = np.linalg.norm(delta)
        
        if delta_norm > self.max_servo_speed:
            s_smoothed = self.current_servos_logic + delta * (self.max_servo_speed / delta_norm)
        
        # --- 层3: 阻尼因子 ---
        s_damped = self.current_servos_logic * (1 - self.damping_factor) + s_smoothed * self.damping_factor
        
        s_logic_final = np.array([int(round(np.clip(x, 0, 180))) for x in s_damped])
        s_hardware = np.array([s_logic_final[0], s_logic_final[1], 180 - s_logic_final[2], 180 - s_logic_final[3]])
        
        # --- 层4: 死区过滤 ---
        if np.sum(np.abs(s_hardware - self.last_sent_servos)) < 1.0:
            return 

        if self.ser:
            cmd = f"Servo_ArmX{s_hardware[0]}\nServo_ArmY{s_hardware[1]}\n" \
                  f"Servo_ArmZ{s_hardware[2]}\nServo_ArmB{s_hardware[3]}\n"
            self.ser.write(cmd.encode())
            self.last_sent_servos = s_hardware.astype(int)
        
        self.current_servos_logic = s_logic_final # 这里的逻辑值已经是修正后的，保证迭代平滑
        _, _, pts = self.forward_kinematics(*s_logic_final)
        self.path_history.append(pts)

    def move_line(self, start_xyz, end_xyz, p_start=-90, p_end=-90, duration=3.0):
        """核心改进：多层S-Curve轨迹规划 + 动态自适应FPS"""
        distance = np.linalg.norm(end_xyz - start_xyz)
        
        if distance < 50:
            fps = 20  
        elif distance < 150:
            fps = 40  
        else:
            fps = 60  
            
        steps = max(int(duration * fps), 10)  
        print(f"规划平滑路径: {start_xyz} -> {end_xyz}, 距离={distance:.1f}mm, FPS={fps}")
        
        for i in range(steps + 1):
            t = i / steps
            smooth_t = 0.5 * (1 - math.cos(math.pi * t)) 
            
            curr_xyz = start_xyz + (end_xyz - start_xyz) * smooth_t
            curr_pitch = p_start + (p_end - p_start) * smooth_t
            
            best_s = self.inverse_kinematics(curr_xyz, target_pitch=curr_pitch)
            self._send_and_audit(best_s, curr_xyz)
            time.sleep(1.0 / fps)

    def gripper_control(self, angle):
        if self.ser:
            self.ser.write(f"Servo_Gripper{int(angle)}\n".encode())
            time.sleep(0.8) 

    def set_damping_params(self, buffer_size=3, max_speed=30.0, damping_factor=0.7):
        self.buffer_size = buffer_size
        self.max_servo_speed = max_speed
        self.damping_factor = damping_factor
        print(f"✓ 减震参数已更新: 滤波窗口={buffer_size}, 限速={max_speed}度/帧, 阻尼={damping_factor}")

    # --- 【新增】设置修正系数的接口 ---
    def set_correction(self, offset_y=0.0, offset_z=0.0):
        self.OFFSET_Y = offset_y
        self.OFFSET_Z = offset_z
        print(f"✓ 倾斜修正已更新: Y_OFFSET={offset_y}, Z_OFFSET={offset_z}")

    def close(self):
        if self.ser: self.ser.close()

if __name__ == "__main__":
    arm = RobotArmUltimate(port='COM3')
    
    # 1. 设置减震参数
    arm.set_damping_params(buffer_size=3, max_speed=30.0, damping_factor=0.7)

    # 2. 【核心调试】在这里设置修正值
    # 如果正向移动时 Z 往下掉，说明大臂（Y）低了，给它加一点
    arm.set_correction(offset_y=-10.0, offset_z=0.0) 
    
    p_standby = np.array([110, 100, 20]) 
    p_pick1   = np.array([210, 110, 20]) 
    p_pick2   = np.array([210, -110, 20]) 
    p_drop    = np.array([110, -100, 20]) 

    try:
        print("\n=== 开始平滑动作序列 (带倾斜修正) ===")
        
        # 1. 初始定位
        # s_init = arm.inverse_kinematics(p_standby, target_pitch=-90)
        # arm._send_and_audit(s_init, p_standby)
        # time.sleep(2)

        # 2. 减震下降
        arm.move_line(p_standby, p_pick1, p_start=-60, p_end=-90, duration=1.0)
        arm.gripper_control(120)

        # 3. 减震平移
        arm.move_line(p_pick1, p_pick2, p_start=-90, p_end=-30, duration=1.0)
        time.sleep(1)
        arm.move_line(p_pick2, p_drop, p_start=-30, p_end=-90, duration=1.0)
        arm.gripper_control(70)
        time.sleep(1)

        # 4. 减震返回待命
        arm.move_line(p_drop, p_standby, p_start=-90, p_end=-60, duration=1.0)
        time.sleep(1)

        print("\n>>> 任务结束。")

    except KeyboardInterrupt:
        pass
    finally:
        if arm.ser: arm.ser.close()