1. 矩阵q要先定义大小，再赋值。不可以直接赋值。下面这种方式是错误的Eigen::MatrixXd q；q<<1,2；正确的这样的：

Eigen::MatrixXd q(2,1);
q<<1.4, 1.5;

2.  不要重复加载variables.h头文件，这样变量会被赋值两次报错。就是说如果all_inlcudes.h里面是项目的所有库文件，你在all_inlcudes.h文件里面加载了变量命名空间variables.h和variables.cpp，那么久不要重新再main.cpp里面再加载一遍variables.h和variables.cpp了。

3. 大循环的迭代变量用了i，在小循环里面就不要再用变量i了，不然就会报错。

4.用matlab计算逆矩阵的时候，最大奇异值的范围和施加的需要好好设计，比如下面的50和0.005，都需要选取合适的参数大小，不然有可能导致系统发散

    M_inv = pinv(M);                                                        % 机械臂惯性矩阵求逆if norm(M_inv,2) > 50                                                % ===不确定要不要加===% 防止矩阵出现奇异 M_inv = M'*pinv(M*M'+0.005*eye(size(M,1)));en

5.更换模型之后，要更改的就两个地方，一个是矩阵求逆的参数设置，一个就是PID和Dx和Kx的参数设置 

6. 如果在franka的desk桌面上设置了末端有夹爪，那么需要注意下面的情况：

• 机械臂的关节力矩就没有末端夹爪的重力补偿，在示教模式下机械臂会下坠；
• 在机械臂运动的时候会进行碰撞检测，检测末端夹爪是否与机械臂本体相碰撞。所以如果你末端没有安装夹爪并且在Franka桌面设置了夹爪，那么有可能机械臂虚空的夹爪检测到会与本体碰撞就会停下来。
• 注意！！！！机械臂末端并没有与桌面进行的碰撞检测。除非设置了robot.setCollisionBehavior，这样也只有真的与桌面碰撞了关节力矩突然变大超限才会停下。
• 在robot_state.O_T_EE中，通过franka内置函数得到机械臂末端位姿，他会根据用户在desk桌面上设置的是否有末端夹爪进行补偿。

所以，要注意！！！第一：一定要注意desk桌面上到底有没有设置存在末端夹爪。

第二：最好使用自己写的Frank正运动学计算。反正就是franka自带的函数会根据是否有夹爪自动调节输出。

Matlab犯错的点

1. Franka机械臂计算正运动学的时候注意，Flange=0.107不是代表机械臂自带的夹爪的高度，而是最最后一个关节，也就是与夹爪相连的那个关节的长度。记住如果末端要安装机械臂的夹爪，需要另外增加一个末端执行器的T_EndEffector。下面给出Franka机械臂的正运动学代码。

   #include <Eigen/Core>#include<iostream>using namespace Eigen;/*** 根据给定的关节角度、Franka相关参数以及末端执行器相关参数，计算 Franka Emika 机械臂的正向运动学，得到包含欧拉角和位置信息的矩阵* @param[in] q 表示机械臂的关节角度矩阵，维度为 7x1（假设每一行代表一个关节角度）* @param[in] EndEffector 表示末端安装的夹爪的高度（注意！！！Franka默认的配备夹爪高度也要算在EndEffector里面，因为flange=0.107不是指代Franka夹爪）* @return 返回一个 6x1 的矩阵，其中包含计算得到的欧拉角（前三个元素）和末端位置信息（后三个元素），*         表示机械臂末端的姿态和位置。如果函数执行过程中没有出现错误，将正常返回该矩阵。*/
Eigen::MatrixXd FK_Franka_Emika(Eigen::MatrixXd q, double EndEffector)
{double q1 = q(0, 0), q2 = q(1, 0), q3 = q(2, 0), q4 = q(3, 0), q5 = q(4, 0), q6 = q(5, 0), q7 = q(6, 0);Matrix4d T1;T1 << cos(q1), -sin(q1), 0, 0,sin(q1), cos(q1), 0, 0,0, 0, 1, 0.333,0, 0, 0, 1;Matrix4d T2;T2 << cos(q2), -sin(q2), 0, 0,0, 0, 1, 0,-sin(q2), -cos(q2), 0, 0,0, 0, 0, 1;Matrix4d T3;T3 << cos(q3), -sin(q3), 0, 0,0, 0, -1, -0.3160,sin(q3), cos(q3), 0, 0,0, 0, 0, 1;Matrix4d T4;T4 << cos(q4), -sin(q4), 0, 0.0825,0, 0, -1, 0,sin(q4), cos(q4), 0, 0,0, 0, 0, 1;Matrix4d T5;T5 << cos(q5), -sin(q5), 0, -0.0825,0, 0, 1, 0.3840,-sin(q5), -cos(q5), 0, 0,0, 0, 0, 1;Matrix4d T6;T6 << cos(q6), -sin(q6), 0, 0,0, 0, -1, 0,sin(q6), cos(q6), 0, 0,0, 0, 0, 1;Matrix4d T7;T7 << cos(q7), -sin(q7), 0, 0.0880,0, 0, -1, 0,sin(q7), cos(q7), 0, 0,0, 0, 0, 1;Matrix4d Tflange;Tflange << 1, 0, 0, 0,0, 1, 0, 0,0, 0, 1,  0.107 ,0, 0, 0, 1;Matrix4d Tendeffector;Tendeffector << 1, 0, 0, 0,0, 1, 0, 0,0, 0, 1,  EndEffector,0, 0, 0, 1;Matrix4d T = T1 * T2 * T3 * T4 * T5 * T6 * T7 * Tflange*Tendeffector;// 计算欧拉角double alpha = atan2(T(1,0), T(0,0));double beta = atan2(-T(2,0), sqrt(std::pow(T(0,0), 2) + std::pow(T(1,0), 2)));double gamma = atan2(T(2,1), T(2,2));Eigen::MatrixXd euler_pos(6, 1);euler_pos << alpha, beta, gamma, T(0,3), T(1,3), T(2,3) ;return euler_pos;
}    

2. 雅可比矩阵的计算也是同上面所叙述，0.107是最后一个关节后面的长度，一定要加上的。Franka默认的夹爪长度是放在EndEffector里面的。（几何雅可比和另外一个雅可比矩阵的区别没有搞清楚）

3. 计算机械臂末端位置的时候，不能通过x_start(5, 0) = x_start(5, 0) - EndEffector;的方式，直接z值减去末端执行器的长度得到，因为有时候机械臂末端是斜的，也会同时影响x和y。所以还是得通过正运动学x_t = FK_Franka_Emika(q, EndEffector);通过乘以末端执行器的矩阵T得到末端位姿。