Universial robot 运动学
1 正运动学：
1.1 DH方法理解
第i个坐标系固连在第i个连杆的左端。轴i固连于i-1杆，在i-1杆的右端。 i坐标系固定在i杆上，随这i杆转动。
每个连杆有四个参数，第i个连杆：
- ai = (沿着Xi轴，从Zi移动到Zi+1的距离) ，即 连杆i的抽象长度！抽象长度由连杆i两端的两个轴：i-1轴和i轴来决定的，两轴的公垂线啊。
- αi= (从Zi旋转到Zi+1的角度，转轴为Xi，右手定则)，即 连杆i与连杆i+1的夹角。
- di = (沿着Zi轴，从Xi-1移动至Xi的距离)， Xi-1和Xi都是垂于Zi的！
- θi = (绕Zi轴，从Xi-1旋转至Xi的角度)，即连杆i-1与连杆i的夹角阿。
前两个参数的作用是建立 i系与i+1系的关系；
后两个参数的作用是建立i-1系与i系的关系； 从而一个连杆的参数起到了承前启后的作用。
因为ai对应距离，通常假设ai大于零，其他的三个参数可正可负。
建立连杆坐标系的步骤：
1：找出各个关节轴，并标出其延长线。
2：如果轴i和i+1不相交，不平行，则找出关节轴i和i+1的公垂线，并以该公垂线与轴i的交点作为i系的原点。
如果两个轴相交，则以交点作为i系的原点.
如果两个轴平行，则寻找令di=0的公垂线作为Xi，Xi与轴的交点为i系的原点。（让di=0只是为了方便）
3：指定Zi沿着轴向，方向随便。
4：规定Xi沿着公垂线的指向，从Zi指向Zi+1, ;如果Zi和Zi+1相交，则Xi垂直于相交轴的平面，方向随便。
需要注意的是，Dh方法建立的坐标系并不是唯一的，首先 Zi是沿着轴向，方向可以随便选；在关节轴相交的情况下(ai=0),由于只要求Xi从Zi指向Zi+1，从而其指向有两种选择； 在关节轴平行的情况下，坐标系i的原点可以随便选择，因为平行轴的公垂线(Xi)有无数条，通常选择令di=0的Xi。另外，当关节为移动关节时，坐标系的选择也有一定的任意性。
DH方法建立坐标时，有一个原则：尽量使连杆参数为0（在符合其规定的几个原则的情况下）。
相邻坐标系之间变换矩阵的推导：
为求得坐标系i和坐标系i+1之间的关系，建立三个中间坐标系，P，Q，R.
Ti-1绕自己的x轴旋转旋转αi-1, 到达R系；（使Zi-1与Zi平行） --- 右手定则
R系沿着自己的x轴正方向移动ai-1 到达Q系；
Q系绕自己的Z轴旋转θi到达P系；--- 右手定则
P系沿着自己的Z轴正方向移动di到达Ti系；
则i系相对于i-1系的变换矩阵(i-1系到i系的变换)： T(i-1,i) = T(i-1,R)*T(R,Q)*T(Q,P)*T(P,i)
T(i-1,R) = Rx(αi-1) =
T(R,Q)= Dx(ai-1) =
T(Q,P)= Rz(θi) =
T(P,i) = Dz(di) =
T(i-1,i) = T(i-1,R)*T(R,Q)*T(Q,P)*T(P,i) =
由ai-1, αi-1, θi，di决定！
1.2 UR 6轴机器人的正运动学
各轴角度都是0的状态：
直观地看一下6个轴：
建立DH坐标系：-- 来自一国外论文<Analytic Inverse Kinematics for the Universal Robots , UR-5/UR-10 Arms>
authosr: Kelsey P. Hawkins, 可以Google， 该论文上的符号有很多错误！！
根据DH参数表可以直接写出T(i-1,i)了，i=1,2,3,4,5,6。
T(0,6) = T(0,1)*T(1,2)*T(2,3)*T(3,4)*T(4,5)*T(5,6);
//////////////////// ///////////////////////// /////////////////////////
曾经的疑问：1和2坐标系的原点重合的话，如何体现2号轴沿着y1方向的长度？
Ans：：2坐标系是固连在2号轴上的，可以将2号轴看作是一个无限延申的刚体，固连在刚体的哪个位置上，从“把空间点在2系中的坐标转换为1系下的坐标”的角度来讲是没有关系的，因为固连在不同点上时，同一个空间点在2系下的局部坐标是不同的哦。可以假象一下，该机械臂只有1 ，2 两个关节！
/////////////////// //////////////////////////// /////////////////////////
My matlab code:
clear
clc
syms alp0 alp1 alp2 alp3 alp4 alp5 alp6;%alpha0-6
syms t0 t1 t2 t3 t4 t5 t6;%theta0-6，6个轴的旋转角度
syms d0 d1 d2 d3 d4 d5 d6;
syms a0 a1 a2 a3 a4 a5 a6;
T01=[cos(t1) -sin(t1) 0 a0;sin(t1)*cos(alp0) cos(t1)*cos(alp0) -sin(alp0) -sin(alp0)*d2;sin(t1)*sin(alp0) cos(t1)*sin(alp0) cos(alp0) cos(alp0)*d1;0 0 0 1];
T12=[cos(t2) -sin(t2) 0 a1;sin(t2)*cos(alp1) cos(t2)*cos(alp1) -sin(alp1) -sin(alp1)*d2;sin(t2)*sin(alp1) cos(t2)*sin(alp1) cos(alp1) cos(alp1)*d2;0 0 0 1];
T23=[cos(t3) -sin(t3) 0 a2;sin(t3)*cos(alp2) cos(t3)*cos(alp2) -sin(alp2) -sin(alp2)*d3;sin(t3)*sin(alp2) cos(t3)*sin(alp2) cos(alp2) cos(alp2)*d3;0 0 0 1];
T34=[cos(t4) -sin(t4) 0 a3;sin(t4)*cos(alp3) cos(t4)*cos(alp3) -sin(alp3) -sin(alp3)*d4;sin(t4)*sin(alp3) cos(t4)*sin(alp3) cos(alp3) cos(alp3)*d4;0 0 0 1];
T45=[cos(t5) -sin(t5) 0 a4;sin(t5)*cos(alp4) cos(t5)*cos(alp4) -sin(alp4) -sin(alp4)*d5;sin(t5)*sin(alp4) cos(t5)*sin(alp4) cos(alp4) cos(alp4)*d5;0 0 0 1];
T56=[cos(t6) -sin(t6) 0 a5;sin(t6)*cos(alp5) cos(t6)*cos(alp5) -sin(alp5) -sin(alp5)*d6;sin(t6)*sin(alp5) cos(t6)*sin(alp5) cos(alp5) cos(alp5)*d6;0 0 0 1];
T06 = T01*T12*T23*T34*T45*T56;
%用具体数值代替一些符号
T01_num=subs(T01,a0,0); T01_num=subs(T01_num,alp0,0); T01_num=subs(T01_num,t1,t1);T01_num=subs(T01_num,d1,d1);
T12_num=subs(T12,a1,0); T12_num=subs(T12_num,alp1,pi/2); T12_num=subs(T12_num,t2,t2);T12_num=subs(T12_num,d2,0);
T23_num=subs(T23,a2,a2);T23_num=subs(T23_num,alp2,0); T23_num=subs(T23_num,t3,t3);T23_num=subs(T23_num,d3,0);
T34_num=subs(T34,a3,a3);T34_num=subs(T34_num,alp3,0); T34_num=subs(T34_num,t4,t4);T34_num=subs(T34_num,d4,d4);
T45_num=subs(T45,a4,0); T45_num=subs(T45_num,alp4,pi/2); T45_num=subs(T45_num,t5,t5);T45_num=subs(T45_num,d5,d5);
T56_num=subs(T56,a5,0); T56_num=subs(T56_num,alp5,-pi/2);T56_num=subs(T56_num,t6,t6);T56_num=subs(T56_num,d6,d6);
T06_num = T01_num*T12_num*T23_num*T34_num*T45_num*T56_num
%化简各个表达式：
T06_num_sim = simplify(T06_num)
%求逆解用到的一些参数：
syms Nx Ny Nz Ox Oy Oz Ax Ay Az Px Py Pz
T06_known = [Nx Ox Ax Px;Ny Oy Ay Py;Nz Oz Az Pz;0 0 0 1]%已知末端位姿求各轴角度
%求解theta1 5 6：
T01_num_inv=simplify( inv(T01_num) );
T56_num_inv=simplify( inv(T56_num) );
Left = simplify( T01_num_inv*T06_known*T56_num_inv )
T15_num= simplify( T12_num*T23_num*T34_num*T45_num )
%求解theta2 3 4
T45_num_inv = simplify( inv(T45_num) );
Left_2 = simplify( T01_num_inv*T06_known*T56_num_inv*T45_num_inv )
T14_num= simplify( T12_num*T23_num*T34_num)
结果，
T01_num =
[ cos(t1), -sin(t1), 0, 0]
[ sin(t1), cos(t1), 0, 0]
[ 0, 0, 1, d1]
[ 0, 0, 0, 1]
T12_num =
[ cos(t2), -sin(t2), 0, 0]
[ 0, 0, -1, 0]
[ sin(t2), cos(t2), 0, 0]
[ 0, 0, 0, 1]
T23_num =
[ cos(t3), -sin(t3), 0, a2]
[ sin(t3), cos(t3), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T34_num =
[ cos(t4), -sin(t4), 0, a3]
[ sin(t4), cos(t4), 0, 0]
[ 0, 0, 1, d4]
[ 0, 0, 0, 1]
T45_num =
[ cos(t5), -sin(t5), 0, 0]
[ 0, 0, -1, -d5]
[ sin(t5), cos(t5), 0, 0]
[ 0, 0, 0, 1]
T56_num =
[ cos(t6), -sin(t6), 0, 0]
[ 0, 0, 1, d6]
[ -sin(t6), -cos(t6), 0, 0]
[ 0, 0, 0, 1]进一步化简T06_num_sim：
//matlab数组的下标是从1开始的
//旋转矩阵
T06_num_sim(1,1)=c6s1s5 + c1c5c6c234 - c1s6s234
T06_num_sim(2,1)=c5*c6*c234*s1 - s1*s6*s234 - c1*c6*s5
T06_num_sim(3,1)=c234s6+s234c5c6
T06_num_sim(1,2)=-c1c6s234 - s1s5s6 - s6c1c234c5
T06_num_sim(2,2)=c1*s5*s6 - c6*s1*s234 - c5*c234*s1*s6
T06_num_sim(3,2)=c234c6-s234c5s6
T06_num_sim(1,3)=c5s1 - c1c234s5
T06_num_sim(2,3)=-c1*c5 - c234*s1*s5
T06_num_sim(3,3)=-s5*s234
//末端位置
T06_num_sim(1,4)= -( a3*c1*s2*s3 - a2*c1*c2 - c5*d6*s1 - c1*d5*s234 - d4*s1 - a3*c1*c2*c3 + c1*c234*d6*s5 )
T06_num_sim(2,4)=-( c1*d4 + c1*c5*d6 - a2*c2*s1 - d5*s1*s234 + c234*d6*s1*s5 + a3*s1*s2*s3 - a3*c2*c3*s1 )
T06_num_sim(3,4)=d1-d5c234+a3s23+a2s2-d6s5s234
用上述方法建模求得的最后结果的末端位置px py pz的值与git上的UR代码计算的差一个负号，原因还没有探索；另外的3*3旋转矩阵的计算结果与git UR上是否一致还没有检查。
git UR: https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_kinematics/src/ur_kin.cpp 中的ur_kinematics函数。
2 运动学逆解：
运动学逆解有数值迭代法，解析法。解析法 包括矢量代数法、几何法、矩阵法和四元数法等，优点是可以得到全部解，缺点在于难度较
大，方法通用性不强。数值解法直接求解约束方程组，可以通过迭代运算求得任何机构的实数解，但通常不能得到全部解，一般而言，初值的选取及搜索算法对收敛性和精度影响较大。
相比传统的六自由度机器人，UR 机器人具有各个关节都能整周回转的优点，且由于UR机器人机构特点满足机器人机构学的 Pieper 准则，因此其运动学逆解具有封闭解的优势，使运动控制更为容易。
Pieper准则是：机器人的三个相邻关节轴交于一点或三轴线平行。
对于6自由度的机器人来说，运动学反解非常复杂，一般没有封闭解(就是解析解)。Pieper方法就是在此基础上进行研究发现，如果机器人满足两个充分条件中的一个，就会得到解析解，这两个条件是：
（1）三个相邻关节轴相交于一点；
（2）三个相邻关节轴相互平行。
现在的大多数商品化机器人都满足封闭解的两个充分条件之一。如PUMA和STANFORD机器人满足第一条件，而ASEA和MINIMOVER机器人满足第二条件。以PUMA560机器人为例，它的最后3个关节轴相交于一点。我们运用Pieper方法解出它的封闭解，从求解的过程中我们也可以发现，这种求解方法也适用于带有移动关节的机器人。
先看数学基础：
2.1 两个简单的数学方法
这个逆运动学算法求解的角度范围是
因为标准的反正切arctan的值域是
所以不能使用，这里介绍一个改进的反正切求法 Atan2(y, x)（Matlab里有这个函数）,它的值域可以满足要求。
C 语言里 double atan2(double y, double x) 返回的是原点至点(x,y)的方位角，即与 x 轴的夹角。也可以理解为复数 x+yi 的辐角。返回值的单位为弧度，取值范围为(-pi, pi]。在数学坐标系中，结果为正表示从 X 轴逆时针旋转的角度，结果为负表示从 X 轴顺时针旋转的角度。 要注意的是，函数atan2(y,x)中参数的顺序是倒置的，atan2(y,x)计算的值相当于点(x,y)的角度值。
2.1.2 解方程
首先进行三角恒等变换，令
其中：
然后带入原方程：
则
上式中d3应该是d;
前提应该是{ Px*Px + Py * Py - d*d >= 0 且 Px*Px + Py * Py >0} 在该前提下解才存在。
2.2 解析法求解逆运动学（用矩阵的变换）：
T(0,6)= T(0,1)*T(1,2)*T(2,3)*T(3,4)*T(4,5)*T(5,6);//这里T(i,i+1)表示i系至i+1系的变换矩阵
记已知末端位姿矩阵 T_know(0,6) =
[ Nx, Ox, Ax, Px]
[ Ny, Oy, Ay, Py ]
[ Nz, Oz, Az, Pz]
[ 0, 0, 0, 1]
求解关节角theta1,5,6:
2.2.1 求解第一关节角theta1:
变换公式: inv(T(0,1)) * T_know(0,6) * inv(*T(5,6)) = T(1,2)*T(2,3)*T(3,4)*T(4,5) //inv表示求逆阵 ------------------------（1）
式子（1）左侧 =
[ cos(t6)*(Nx*cos(t1) + Ny*sin(t1)) - sin(t6)*(Ox*cos(t1) + Oy*sin(t1)), Ax*cos(t1) + Ay*sin(t1), - cos(t6)*(Ox*cos(t1) + Oy*sin(t1)) - sin(t6)*(Nx*cos(t1) + Ny*sin(t1)), Px*cos(t1) - d6*(Ax*cos(t1) + Ay*sin(t1)) + Py*sin(t1)]
[ cos(t6)*(Ny*cos(t1) - Nx*sin(t1)) - sin(t6)*(Oy*cos(t1) - Ox*sin(t1)), Ay*cos(t1) - Ax*sin(t1), - cos(t6)*(Oy*cos(t1) - Ox*sin(t1)) - sin(t6)*(Ny*cos(t1) - Nx*sin(t1)), Py*cos(t1) - d6*(Ay*cos(t1) - Ax*sin(t1)) - Px*sin(t1)]
[ Nz*cos(t6) - Oz*sin(t6), Az, - Oz*cos(t6) - Nz*sin(t6), Pz - d1 - Az*d6]
[ 0, 0, 0, 1]
式子（1）右侧 =
[ cos(t2 + t3 + t4)*cos(t5), -cos(t2 + t3 + t4)*sin(t5), sin(t2 + t3 + t4), a3*cos(t2 + t3) + a2*cos(t2) + d5*sin(t2 + t3 + t4)]
[ -sin(t5), -cos(t5), 0, -d4]
[ sin(t2 + t3 + t4)*cos(t5), -sin(t2 + t3 + t4)*sin(t5), -cos(t2 + t3 + t4), a3*sin(t2 + t3) + a2*sin(t2) - d5*cos(t2 + t3 + t4)]
[ 0, 0, 0, 1]式子（1）左右两侧的第二行最后一列相等：
Py*c1 - d6*(Ay*c1 - Ax*s1) - Px*s1 = -d4
设m = (d6*Ay - Py), n = d6*Ax -Px,
则 m*c1 - n*s1 = d4
根据前面介绍的解方程的方法：
2.2.1 求解theta5:
式子（1）左右两侧的第2行,第2列相等：
Ax*s1 - Ay*c1 = c5
从而 theta5 =+arccos(Ax*s1 - Ay*c1) 或 - arccos(Ax*s1 - Ay*c1)
2.2.1 求解theta6:
式子（1）左右两侧的第2行,第1列相等：
-s5 = c6*(Ny*c1 - Nx*s1) - s6*(Oy*c1 - Ox*s1)
记 Ny*c1 - Nx*s1 = n; Oy*c1 - Ox*s1 = m
则-s5 = c6*n - s6*m
带入公式得
theta6 = atan2(n,m) - atan2(-s5, 0) -----(根据旋单位转矩阵的性质，m*m+n*n-s5*s5=0)
= atan2(n,m) + atan2(s5, 0)
根据前面公式，解存在的条件是m*m+n*n-s5*s5 >= 0 且 m*m+n*n > 0， 这里已经是m*m+n*n-s5*s5=0，所以解存在的条件是s5*s5>0 即 s5不等于0；
然后，一般的资料上会进一步写成：theta6 = atan2(n/s5,m/s5) （可以成立吗？？？）
对于atan2(y,x),当 x与y同时为0时，无解，写成这样是为了说明s5=0时，解不存在。
（--------atan2()的性质，画图可知 atan2(y,x) = -atan2(-y,x)；）
求解2，3，4关节角
变换公式: inv(T(0,1)) * T_know(0,6) * inv(*T(5,6))* inv(*T(4,5)) = T(1,2)*T(2,3)*T(3,4) //inv表示求逆阵 ------------------------（2）
式子（2）左侧 =
[ cos(t5)*(cos(t6)*(Nx*cos(t1) + Ny*sin(t1)) - sin(t6)*(Ox*cos(t1) + Oy*sin(t1))) - sin(t5)*(Ax*cos(t1) + Ay*sin(t1)), cos(t6)*(Ox*cos(t1) + Oy*sin(t1)) + sin(t6)*(Nx*cos(t1) + Ny*sin(t1)), sin(t5)*(cos(t6)*(Nx*cos(t1) + Ny*sin(t1)) - sin(t6)*(Ox*cos(t1) + Oy*sin(t1))) + cos(t5)*(Ax*cos(t1) + Ay*sin(t1)), Px*cos(t1) - d6*(Ax*cos(t1) + Ay*sin(t1)) + d5*(cos(t6)*(Ox*cos(t1) + Oy*sin(t1)) + sin(t6)*(Nx*cos(t1) + Ny*sin(t1))) + Py*sin(t1)]
[ cos(t5)*(cos(t6)*(Ny*cos(t1) - Nx*sin(t1)) - sin(t6)*(Oy*cos(t1) - Ox*sin(t1))) - sin(t5)*(Ay*cos(t1) - Ax*sin(t1)), cos(t6)*(Oy*cos(t1) - Ox*sin(t1)) + sin(t6)*(Ny*cos(t1) - Nx*sin(t1)), sin(t5)*(cos(t6)*(Ny*cos(t1) - Nx*sin(t1)) - sin(t6)*(Oy*cos(t1) - Ox*sin(t1))) + cos(t5)*(Ay*cos(t1) - Ax*sin(t1)), Py*cos(t1) - d6*(Ay*cos(t1) - Ax*sin(t1)) + d5*(cos(t6)*(Oy*cos(t1) - Ox*sin(t1)) + sin(t6)*(Ny*cos(t1) - Nx*sin(t1))) - Px*sin(t1)]
[ cos(t5)*(Nz*cos(t6) - Oz*sin(t6)) - Az*sin(t5), Oz*cos(t6) + Nz*sin(t6), Az*cos(t5) + sin(t5)*(Nz*cos(t6) - Oz*sin(t6)), Pz - d1 - Az*d6 + d5*(Oz*cos(t6) + Nz*sin(t6))]
[ 0, 0, 0, 1]式子（2）右侧 =
[ cos(t2 + t3 + t4), -sin(t2 + t3 + t4), 0, a3*cos(t2 + t3) + a2*cos(t2)]
[ 0, 0, -1, -d4]
[ sin(t2 + t3 + t4), cos(t2 + t3 + t4), 0, a3*sin(t2 + t3) + a2*sin(t2)]
[ 0, 0, 0, 1]-----求解关节角3 (此时已经解出了 theta1 5 6 )
左右两侧矩阵的第1行第4列相等：
Px*c1 - d6*(Ax*c1+Ay*s1) + d5*( c6*(Ox*c1+Oy*s1) + s6*(Nx*c1+Ny*s1) ) +Py*s1 = a3*c23 + a2*c2 -------------------(3)
左右两侧矩阵的第3行第4列相等：
Pz - d1 - Az*d6 + d5*(Oz*c6 + Nz*s6) = a3*s23 + a2*s2 -----------------------------------------------------------------------------(4)
由于theta1，5，6在前面已经解出来了，(3),(4)式左侧可以计算出来，记(3)式左侧为m, (4)式左侧为n, 则：
m = a3*c23 + a2*c2------------------------------------------------------------------------------------------------------------------------------ (5)
n = a3*s23 + a2*s2 -------------------------------------------------------------------------------------------------------------------------------(6)
(5)(6)两式平方相加：
m*m + n*n = a2*a2 + a3*a3 + 2*a2*a3*(c2*c23 + s2*s23)
= a2*a2 + a3*a3 + 2*a2*a3*c3
从而 theta3 = +/- arccos[ (m*m + n*n - a2*a2 - a3*a3 )/(2*a2*a3) ] ;
前提：[ ]内部的表达式绝对值要小于等于1，即： (m*m + n*n) <= (a2 + a3)^2;
-----求解关节角2 (此时已经解出了 theta1 5 6 3)
将(5)式展开： m = c2*(a3*c3 + a2) - s2*a3*s3 -----------------------------------------------------------------------------------------(7)
将(6)式展开： n = c2*(a3*s3) + s2*(a2 + a3*c3)---------------------------------------------------------------------------------------(8)
将s2，c2看作未知数，可以解先线性方程得到：
s2 = [ ( a2*c3 + a2)*n - a3*s3*m ] / (a2*a2 + a3*a3 + 2*a2*a3*c3)
c2 = (m + s2*a3*s3) / (a3*c3 + a2)
从而 theta2 = Atan2(s2, c2)
-----求解关节角4 (此时已经解出了 theta1 5 6 3 2)
(2)式左右两侧第一行第二列相等：
-s234 = c6*(Ox*c1+Oy*s1) + s6*(Nx*c1 + Ny*s1) --- 左侧记作m
c234 = Oz*c6 + Nz*s6 ---------------------------------------- 左侧记作n
从而 theta4 = Atan2(-m,n) - theta2 -theta3
整理求解公式：
theta1: m = (d6*Ay - Py), n = d6*Ax -Px,
theta5: +arccos(Ax*s1 - Ay*c1) 或 - arccos(Ax*s1 - Ay*c1)
theta6:
theta6 = atan2(n,m) + atan2(s5, 0) --- 解存在的条件是s5*s5>0 即 s5不等于0；
Ny*c1 - Nx*s1 = n; Oy*c1 - Ox*s1 = m
然后，一般的资料上会进一步写成：theta6 = atan2(n/s5,m/s5) （可以成立吗？？？）
theta3 = +/- arccos[ (m*m + n*n - a2*a2 - a3*a3 )/(2*a2*a3) ] ;
前提：[ ]内部的表达式绝对值要小于等于1，即： (m*m + n*n) <= (a2 + a3)^2;
--------------------------------------------------------------------------------------------
theta2 = Atan2(s2, c2)
s2 = [ ( a2*c3 + a2)*n - a3*s3*m ] / (a2*a2 + a3*a3 + 2*a2*a3*c3)
c2 = (m + s2*a3*s3) / (a3*c3 + a2)
theta4 = Atan2(-m,n) - theta2 -theta3
-s234 = c6*(Ox*c1+Oy*s1) + s6*(Nx*c1 + Ny*s1) --- 左侧记作m
c234 = Oz*c6 + Nz*s6 ---------------------------------------- 左侧记作n
奇异位置：
theta1:
matlab robot tool box学习：
%From internet:机器人工具箱中的比运动学函数并不精确，同时机器人通常有多组逆解，而ikine函数只能求出一组。
%对满足pieper条件的机器人，最好自己求出他的解析解，利用解析解来求得多组逆解，不仅速度快，而且更精确。
%尝试了将变量定义成sym，传入Link函数，但是正运动学计算的结果有乱码！
%用SerialLink.ikine Numerical inverse kinematics 求逆解时 我遇到了matlab提示迭代错误，solution diverging at step 304, try reducing alpha。
%帮助文档中提到用SerialLink.ikine_sym可计算符号逆解，但是in experimental --
%感觉这个工具箱的作用最主要是画图画轨迹
clc;
clear;
%startup_rvc;
%syms d1 d4 d5 d6 a2 a3; syms t1 t2 t3 t4 t5 t6;
d1 = 100;d4 = 100;d5 = 100;d6 = 100; a2 = 100;a3 = 100;
% d:link offset, a:link length, alpha:link twist;
%L1 = Link( 'd', d1, 'a', 0, 'alpha', pi/2, 'sigma',0,'mdh',1);
L1 = Link('revolute', 'd', d1, 'a', 0.0, 'alpha', pi/2, 'modified');
L2 = Link('revolute', 'd', 0.0, 'a', a2, 'alpha', 0, 'modified');
L3 = Link('revolute', 'd', 0.0, 'a', a3, 'alpha', 0, 'modified');
L4 = Link('revolute', 'd', d4, 'a', 0.0, 'alpha', pi/2, 'modified');
L5 = Link('revolute', 'd', d5, 'a', 0.0, 'alpha', -pi/2, 'modified');
L6 = Link('revolute', 'd', d6, 'a', 0.0, 'alpha', 0, 'modified');%a 和alpha都为零，因为后面没有轴了。
robot=SerialLink([L1 L2 L3 L4 L5 L6]);
robot.name = 'abc';
robot.display()
thetas = [1.8 1 1.6 1 0.3 0.6];
pos1 = robot.fkine(thetas) %forward kinematics
%robot.fkine([t1 t2 t3 t4 t5 t6])
Q0=[1 1 1 1 1 1];
%thetas1 = robot.ikine(pos1,Q0) %逆运动学，该函数接受初始估计值Q0作为迭代的开始,第三个参数还可以指定步长等。
%robot.plot([pi/4 pi/3 pi/3 pi/3 pi/3 pi/3])%画出机械臂的形态
for i=1:1:50
%robot.plot([i 2*i 3*i pi/3 pi/3 pi/3])
pos = robot.fkine([i 2*i 3*i pi/3 pi/3 pi/3]); %forward kinematics
plot3(pos(1),pos(2),pos(3),'r.','MarkerSize',15.0);%画图, 也可调用robot.plot进行画图，但是运行速度很慢
hold on
end
ref:
https://blog.csdn.net/fengyu19930920/article/details/81144042
