机器人运动学：正解与逆解

T = [ R  p ]      R 为 3×3 旋转矩阵（正交，det=1）
    [ 0  1 ]      p 为 3×1 平移向量

复合：T_0_2 = T_0_1 · T_1_2
逆：  T^-1 = [ R^T  -R^T·p ]
             [ 0     1      ]

#include <Eigen/Dense>
using Eigen::Matrix4d;
using Eigen::Matrix3d;
using Eigen::Vector3d;

// 绕固定轴的变换构造
Matrix4d rotX(double a) {
  Matrix4d T = Matrix4d::Identity();
  T.block<3,3>(0,0) = Eigen::AngleAxisd(a, Vector3d::UnitX()).toRotationMatrix();
  return T;
}
Matrix4d rotZ(double a) {
  Matrix4d T = Matrix4d::Identity();
  T.block<3,3>(0,0) = Eigen::AngleAxisd(a, Vector3d::UnitZ()).toRotationMatrix();
  return T;
}
Matrix4d trans(double x, double y, double z) {
  Matrix4d T = Matrix4d::Identity();
  T.block<3,1>(0,3) = Vector3d(x, y, z);
  return T;
}

标准 DH（Denavit-Hartenberg，1955）：
  a_i    ：沿 x_i 轴，从 z_i 到 z_{i+1} 的距离
  alpha_i：绕 x_i 轴，从 z_i 到 z_{i+1} 的转角
  d_i    ：沿 z_i 轴，从 x_{i-1} 到 x_i 的距离
  theta_i：绕 z_i 轴，从 x_{i-1} 到 x_i 的转角（转动关节的变量）

变换矩阵（标准型）：
  A_i = Rot_z(theta_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(alpha_i)

改进 DH（Craig，Modified DH）：
  把坐标系固定在连杆近端而非远端，变换顺序不同
  A_i = Rot_x(alpha_{i-1}) · Trans_x(a_{i-1}) · Rot_z(theta_i) · Trans_z(d_i)

Matrix4d dhTransform(double a, double alpha, double d, double theta) {
  // 标准 DH：Rot_z(theta) * Trans_z(d) * Trans_x(a) * Rot_x(alpha)
  return rotZ(theta) * trans(0, 0, d) * trans(a, 0, 0) * rotX(alpha);
}

struct DH {
  double a, alpha, d, theta_offset;
  int type;   // 0 = revolute, 1 = prismatic
};

Matrix4d forwardKinematics(const std::vector<DH> & chain,
                           const std::vector<double> & q) {
  Matrix4d T = Matrix4d::Identity();
  for (size_t i = 0; i < chain.size(); ++i) {
    const DH & l = chain[i];
    double theta = (l.type == 0) ? (q[i] + l.theta_offset) : l.theta_offset;
    double d = (l.type == 1) ? (q[i] + l.d) : l.d;
    T = T * dhTransform(l.a, l.alpha, d, theta);
  }
  return T;
}

import numpy as np

def dh(a, alpha, d, theta):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0.0,    sa,     ca,    d],
        [0.0,  0.0,    0.0,  1.0],
    ])

def fk(chain, q):
    T = np.eye(4)
    for (a, alpha, d, off), qi in zip(chain, q):
        T = T @ dh(a, alpha, d, qi + off)
    return T

<joint name="shoulder_pan_joint" type="revolute">
  <parent link="base_link"/>
  <child link="shoulder_link"/>
  <origin xyz="0 0 0.089159" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit lower="-6.2832" upper="6.2832" effort="150" velocity="3.15"/>
</joint>

几何法（对每个关节 i）：
  转动关节：
    J_v,i = z_{i-1} × (p_e - p_{i-1})
    J_w,i = z_{i-1}
  移动关节：
    J_v,i = z_{i-1}
    J_w,i = 0

  其中 z_{i-1} 是第 i-1 个关节轴在世界系中的方向
       p_{i-1} 是第 i-1 个坐标系原点在世界系中的位置
       p_e 是末端位置

// 用 Pinocchio 求雅可比，避免手写推导出错
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>

pinocchio::Model model;
pinocchio::urdf::buildModel("arm.urdf", model);
pinocchio::Data data(model);

Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
pinocchio::computeJointJacobians(model, data, q);
pinocchio::forwardKinematics(model, data, q);

Eigen::Matrix<double, 6, Eigen::Dynamic> J(6, model.nv);
J.setZero();
pinocchio::getJointJacobian(model, data,
    model.getJointId("tool0"), pinocchio::WORLD, J);

Eigen::MatrixXd jacobianNumerical(FkFn fk, const Eigen::VectorXd & q,
                                  double eps = 1e-6) {
  const int n = q.size();
  const Eigen::Matrix4d T0 = fk(q);
  const Eigen::Matrix3d R0 = T0.block<3,3>(0,0);
  const Eigen::Vector3d p0 = T0.block<3,1>(0,3);
  Eigen::MatrixXd J(6, n);
  for (int i = 0; i < n; ++i) {
    Eigen::VectorXd qp = q; qp[i] += eps;
    Eigen::Matrix4d T1 = fk(qp);
    J.block<3,1>(0,i) = (T1.block<3,1>(0,3) - p0) / eps;
    // 角速度：用旋转矩阵的反对称部分提取
    Eigen::Matrix3d dR = (T1.block<3,3>(0,0) - R0) / eps;
    Eigen::Matrix3d W = dR * R0.transpose();
    J(3,i) = W(2,1); J(4,i) = W(0,2); J(5,i) = W(1,0);
  }
  return J;
}

典型 6R 臂的解析步骤（球形手腕）：
  1. 用腕心位置解前三个关节
     腕心 p_wc = p_e - d6 · R_e · z_6
     θ1 = atan2(p_wc_y, p_wc_x) ± π/2 修正
     θ3 由余弦定理：cos θ3 = (r² - a2² - a3²) / (2·a2·a3)
     θ2 = atan2(...) - atan2(...)
  2. 由 R_3_6 = R_0_3^T · R_e 解后三个关节（欧拉角分解）
     θ4, θ5, θ6 从 ZYZ 型旋转矩阵提取
  3. 每个 θ1 分支对应两组解，共 8 组（受关节限位过滤后通常剩 2~4 组）

def ik_ur5(T_target, d1=0.089159, a2=-0.425, a3=-0.39225,
           d4=0.10915, d5=0.09465, d6=0.0823):
    """返回所有满足关节限位的解（最多 8 组）"""
    R = T_target[:3, :3]
    p = T_target[:3, 3]
    # 腕心位置
    p_wc = p - d6 * R[:, 2]
    sols = []
    for s1 in (+1, -1):
        theta1 = np.arctan2(p_wc[1], p_wc[0]) + s1 * np.pi / 2
        r = np.hypot(p_wc[0], p_wc[1])
        if abs(p_wc[2] - d1) > (abs(a2) + abs(a3)):
            continue                       # 超出工作空间，跳过该分支
        cos3 = (r**2 - a2**2 - a3**2) / (2 * a2 * a3)
        if abs(cos3) > 1.0:
            continue                       # 不可达
        for s3 in (+1, -1):
            theta3 = s3 * np.arccos(np.clip(cos3, -1, 1))
            # ... 由几何关系求 theta2，再由 R 求 theta4/5/6
            sols.append((theta1, theta2, theta3, theta4, theta5, theta6))
    return sols

牛顿-拉夫逊迭代：
  1. 计算当前位姿误差：e = [p_target - p(q); 姿态误差]
  2. 求解 J(q) · Δq = e
  3. 更新 q ← q + α · Δq
  4. 若 ||e|| < ε 则收敛，否则重复（上限 100~200 次）

Δq = J^T (J J^T + λ² I)^-1 · e

λ 的取值策略（由 σ_min 决定）：
  σ_min > σ_0        ：λ = 0（正常伪逆）
  σ_min < σ_0        ：λ² = (1 - (σ_min/σ_0)²) · λ_max²
  这样在远离奇异时退化为普通伪逆，在奇异附近平滑地增加阻尼
  典型值：σ_0 = 0.05, λ_max = 0.05

// 阻尼最小二乘逆解迭代
bool ikDLS(const pinocchio::Model & model, pinocchio::Data & data,
           const Eigen::Matrix4d & T_target, Eigen::VectorXd & q,
           double tol = 1e-4, int max_iter = 200) {
  const double lambda = 0.05;
  for (int it = 0; it < max_iter; ++it) {
    pinocchio::forwardKinematics(model, data, q);
    const Eigen::Matrix4d T = data.oMi[model.nv - 1].toHomogeneousMatrix();
    // 位姿误差：位置差 + 旋转的 so(3) 对数映射
    Eigen::Matrix<double, 6, 1> e;
    e.head<3>() = T_target.block<3,1>(0,3) - T.block<3,1>(0,3);
    const Eigen::Matrix3d dR = T_target.block<3,3>(0,0)
                             * T.block<3,3>(0,0).transpose();
    e.tail<3>() = pinocchio::log3(dR);
    if (e.norm() < tol) return true;

    Eigen::Matrix<double, 6, Eigen::Dynamic> J(6, model.nv);
    pinocchio::computeJointJacobians(model, data, q);
    J.setZero();
    pinocchio::getJointJacobian(model, data,
        model.nv - 1, pinocchio::LOCAL, J);

    // Δq = J^T (J J^T + λ²I)^-1 e
    Eigen::Matrix<double, 6, 6> JJt = J * J.transpose();
    JJt += lambda * lambda * Eigen::Matrix<double, 6, 6>::Identity();
    Eigen::Matrix<double, 6, 1> dq = J.transpose() * JJt.ldlt().solve(e);
    q = pinocchio::integrate(model, q, dq);   // 正确的流形积分
  }
  return false;
}

腕部奇异：θ5 ≈ 0 或 π，第 4、6 轴共线
  现象：θ4 与 θ6 有无穷多组合，数值解剧烈跳变
  对策：给 θ5 加一个小偏置（如 ±0.02 rad）绕开

肘部奇异：手臂完全伸直，θ3 ≈ 0
  现象：沿手臂方向的运动不可实现
  对策：限制工作空间，不让目标点落在伸直构型上

肩部奇异：腕心落在第 1 轴线上
  现象：θ1 不确定
  对策：目标点避开基座轴线附近区域

Eigen::JacobiSVD<Eigen::MatrixXd> svd(J);
double sigma_min = svd.singularValues().minCoeff();
double sigma_max = svd.singularValues().maxCoeff();
double cond = sigma_max / sigma_min;      // 条件数

// 分级策略
if (sigma_min > 0.05) {
  // 正常：普通伪逆
} else if (sigma_min > 0.01) {
  // 接近奇异：启用阻尼最小二乘
} else {
  // 严重奇异：拒绝该目标，上报规划层重新选点
}

// 单元测试：正解与 Pinocchio 对比，逆解往返验证
TEST(Kinematics, ForwardMatchesPinocchio) {
  for (int trial = 0; trial < 1000; ++trial) {
    Eigen::VectorXd q = randomConfig();          // 随机位形
    Eigen::Matrix4d T1 = forwardKinematics(chain_, q);
    Eigen::Matrix4d T2 = pinocchioFk(model_, q);
    EXPECT_LT((T1 - T2).norm(), 1e-9);           // 数值级一致
  }
}

TEST(Kinematics, InverseRoundTrip) {
  for (int trial = 0; trial < 1000; ++trial) {
    Eigen::VectorXd q_true = randomConfig();
    Eigen::Matrix4d T = forwardKinematics(chain_, q_true);
    Eigen::VectorXd q_sol;
    ASSERT_TRUE(ikDLS(model_, data_, T, q_sol));
    EXPECT_LT((forwardKinematics(chain_, q_sol) - T).norm(), 1e-6);
  }
}