机器人奇异性 & 奇异区域的判断 make&test

通常来说，一个6轴机械臂有三种个奇异点 (对应三个奇异区域):

1. Wrist Singularities - These happen when two of the robot’s wrist axes (joints 4 and 6) line up with each other. This can cause these joints to try and spin 180 degrees instantaneously.
2. Shoulder Singularities - These happen when the center of the robot’s wrist aligns with the axis of joint 1. It causes joints 1 and 4 to try and spin 180 degrees instantaneously. A subset of this is an Alignment Singularity, where the first and last joints of the robot (joints 1 and 6) line up with each other.
3. Elbow Singularities - These happen when the center of the robot’s wrist lies on the same plane as joints 2 and 3. Elbow singularities look like the robot has “reached too far”, causing the elbow to lock in position. This video shows a good example of an elbow singularity which causes the robot to get stuck.

对于奇异区域的判断：
1. 求得雅克比矩阵
2. 通过svd分解，求得雅克比矩阵的奇异值
3. 判断雅克比矩阵最大特征值和最小特征值的比值是否超过了某个阈值；或者判断雅克比矩阵特征值是否在小于某个接近于零的值 eps；或者判断可操作性—Manipulability是否小于某个阈值： Manipulability=sqrt(det(J’J))=特征值乘积

例如，用 kdl 库求雅克比矩阵的奇异值：

double eps=0.00001;
int maxiter=150;

jnt2jac.JntToJac(q_in,jac);  // 通过关节角求雅克比矩阵

//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
svdResult = svd.calculate(jac,U,S,V,maxiter);
if (0 != svdResult)
{
    std::cerr<<"svd calc error"<<std::endl;
    return;
    // qdot_out.data.setZero();
    // return (error = E_SVD_FAILED);
}

// Use this to check singularity!
std::cerr << "Singular Value:\n";
for (i=0;i<jac.columns();i++)
{
    std::cerr << S(i) << "\t";  // 奇异值, Manipulability=sqrt(S(0)*S(1)*S(2)*...)
}
std::cerr << std::endl;

如果判断机器人处于奇异区域，则不应该通过求逆解来对操作空间插值，而是应该跳过该奇异区域的求逆解过程，直到离开奇异区域之后再求逆解，然后在这个过程中对关节空间插值（不能保证操作空间的运动轨迹）。
例如，yaskawa 的机器人就是牺牲操作空间位置 (或姿态) 的精度来避免出现关节角速度过大的情况。

一般来说，对于操作空间的任意一个位姿，机器人可以有6组解，对应一下几种构型的变换（构型之间存在耦合）
flip—no flip
up—dowm
front—back

从一个构型变换到另外一个构型，不能通过 MOVL或 MOVC 来运动，会经过奇异点。

但是，但是。。。并不是说确定了构型就确定了运动方式。。。
因为，对于末端关节（6轴机器人），即使构型相同，也存在是向左转和向右转的问题。。。只有确定了构型，并且确定了旋转方向，才能唯一确定一个运动形式。

ref link:
http://blog.robotiq.com/why-singularities-can-ruin-your-day
https://www.youtube.com/watch?v=zlGCurgsqg8