def calculate_common_perpendicular(zi, zi_next, pi, pi_next):
    # 异面直线情况
    if skew:
        n = cross(zi, zi_next) / norm(cross(zi, zi_next))
        # 求解线性方程组
        A = column_stack((zi, -zi_next, n))
        b = pi_next - pi
        t, s, _ = lstsq(A, b)
        point1 = pi + t * zi
        point2 = pi_next + s * zi_next
        return (point1, point2)
    
    # 平行线情况
    elif parallel:
        point1 = pi
        point2 = pi_next - dot(pi_next - pi, zi) * zi
        return (point1, point2)

def get_mdh_parameters(chain):
    # ... 前面的计算 ...
    
    # 第一个关节的特殊处理
    if i == 0:
        # Z轴默认为基座的Z轴方向
        mdh_zs.append([0, 0, 1])  # 可以根据需要调整
        # X轴可以选择为基座的X轴方向
        mdh_xs.append([1, 0, 0])  # 任意但需垂直于Z
    
    # 最后一个关节的特殊处理
    if i == num_joints - 1:
        mdh_origins.append(joint_positions[-1])
        mdh_zs.append(joint_vectors[-1])
        # X轴继承前一个关节的X轴方向或根据应用需求选择
        mdh_xs.append(joint_xs[-1])  # 可以根据需要调整

class URDFParser:
    def get_mdh_parameters(self, chain):
        # 获取关节信息
        joint_positions, joint_vectors, joint_xs, joint_types = self.get_joint_axes(chain)
        
        # 过滤固定关节
        active_positions = [pos for i, pos in enumerate(joint_positions) 
                          if joint_types[i] in ['revolute', 'base']]
        active_vectors = [vec for i, vec in enumerate(joint_vectors) 
                        if joint_types[i] in ['revolute', 'base']]
        
        mdh_parameters = []
        
        for i in range(num_joints):
            # 计算MDH原点
            oi, case, _ = self.calculate_mdh_origin_position(
                joint_pos, joint_vector, joint_pos_next, joint_vector_next
            )
            
            # 确定X轴方向
            if case == 'coincident':
                xi = joint_x  # 使用原始X轴
            elif case in ['skew', 'intersect']:
                xi = np.cross(joint_vector, joint_vector_next)
                xi = xi / np.linalg.norm(xi)
            elif case == 'parallel':
                xi = np.cross(joint_vector, perpendicular_direction)
                xi = xi / np.linalg.norm(xi)
            
            # 计算MDH参数
            theta = calculate_theta(x_prev, xi, zi)
            d = calculate_d(o_prev, oi, zi)
            a = calculate_a(o_prev, oi, x_prev)
            alpha = calculate_alpha(z_prev, zi, x_prev)
            
            mdh_parameters.append([theta, d, a, alpha])
        
        return mdh_parameters

def calculate_mdh_origin_position(self, joint_pos, joint_vector, 
                                 joint_pos_next, joint_vector_next):
    """
    计算相邻两个关节坐标系原点oi的位置
    
    返回:
        oi: 坐标系原点位置
        case: 情况分类 ('coincident', 'intersect', 'parallel', 'skew')
        common_perpendicular: 公垂线信息
    """
    
    # 检查两轴是否平行
    if np.allclose(np.cross(zi, zi_next), np.zeros(3)):
        # 检查是否重合
        if on_same_line(zi, zi_next, pi, pi_next):
            return pi, 'coincident', None
        else:
            # 平行但不重合
            return calculate_parallel_case()
    
    # 检查是否相交
    distance = calculate_line_distance(zi, zi_next, pi, pi_next)
    if np.isclose(distance, 0):
        # 相交，找交点
        intersection = find_intersection(zi, zi_next, pi, pi_next)
        return intersection, 'intersect', (intersection, intersection)
    
    # 异面直线
    return calculate_skew_case()

def calculate_angle(v1, v2, axis):
    """
    计算从v1到v2绕axis轴的有向角度
    """
    # 投影到垂直于轴的平面
    v1_proj = v1 - np.dot(v1, axis) * axis
    v2_proj = v2 - np.dot(v2, axis) * axis
    
    # 归一化
    v1_norm = v1_proj / np.linalg.norm(v1_proj)
    v2_norm = v2_proj / np.linalg.norm(v2_proj)
    
    # 计算有向角度
    cos_angle = np.dot(v1_norm, v2_norm)
    sin_angle = np.dot(np.cross(v1_norm, v2_norm), axis)
    
    return np.arctan2(sin_angle, cos_angle)

# URDF定义的关节
joints = [
    {"name": "joint1", "type": "revolute", "axis": [0, 0, 1], 
     "origin": {"xyz": [0, 0, 0.1], "rpy": [0, 0, 0]}},
    {"name": "joint2", "type": "revolute", "axis": [0, 0, 1], 
     "origin": {"xyz": [0.5, 0, 0], "rpy": [0, 0, 0]}}
]

# 计算得到的MDH参数
mdh_parameters = [
    [0, 0.1, 0.5, 0],  # joint1: θ₁=0, d₁=0.1, a₀=0.5, α₀=0
    [0, 0, 0, 0]       # joint2: θ₂=0, d₂=0, a₁=0, α₁=0
]

import roboticstoolbox as rtb
import numpy as np

# 从计算得到的MDH参数创建机器人模型
mdh_config = []
for params in mdh_parameters:
    theta, d, a, alpha = params
    mdh_config.append(rtb.RevoluteMDH(d=d, a=a, alpha=alpha, offset=theta))

robot = rtb.DHRobot(mdh_config, name="test_robot")

# 计算正向运动学
q = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]  # 关节角度
T = robot.fkine(q)

# 与原始URDF的正向运动学结果对比
print("MDH Forward Kinematics:")
print("Position:", T.t)
print("Orientation:", T.R)