作为一名研究机器人方向的研究生，我最近在进行C++的学习，但苦于没有专业项目练手，故考虑使用C++对Matlab上的RoboticToolbox进行移植，并增加一些基于旋量法的操作。一来是为了锻炼自己的C++能力，二来也顺便复习了机器人知识，希望自己能够坚持下来吧！
本文档是我的开发日志和心得体会
开始了移植工作，此时离我完成C++大致学习过了３天，目前将会使用到Eigen这个库来帮助我进行矩阵运算。今天因为有课，还有做项目的关系，只写了个欧拉角求旋转矩阵的函数EulerRot。
这两天断断续续地在写，因为做实验花了很多时间。之前还在思考如何用类、设计模式来设计我的这个机器人库，后来发现这样有点太浪费时间了，不如先以函数的形式写好大部分功能，然后再系统的学习一下设计模式，再开始重构一下代码，合理OOP。新增了以下功能：
-
Skew:求出一个三维向量对应的反对称矩阵，也称为叉乘矩阵，该矩阵可用于向量的叉乘运算:$A\times B=[A]B$，其中$[A]$表示将向量$A$转为反对称矩阵形式； -
RotX, RotY, RotZ:绕单坐标轴旋转的旋转矩阵； -
SE2, SE3:分别为二维和三维的欧几里得群(Special Euclidean group)，即齐次变换矩阵，目前采用的还是欧拉角旋转+位移向量的形式求出齐次变换矩阵，待完善； -
RotMatExp:矩阵指数的方法求出旋转矩阵，其实就是指定一个在被旋转的坐标系下表示的旋转轴向量，然后指定一个绕该轴旋转的旋转角度，即可求出旋转矩阵； -
GetRotAxis:矩阵指数的反运算，矩阵对数。由旋转矩阵反解出矩阵指数所需的旋转轴向量，该向量是在原坐标系下表示的； -
GetRotTheta:矩阵对数，求出绕某一轴旋转的角度，该函数需要配合上一个GetRotAxis函数使用，目前没想到怎么整合它们两个，待完善。
今天就到这里了，周末在家果然不适合学习，再加上导师最近也安排了一些控制理论的学习任务，还是要平衡一下工作节奏。
今天比较忙，上午才慢悠悠返校，下午批改了数据库的作业，顺便学习了一下SQL Server的用法，感觉可以使用数据库存放机械臂信息，然后调用相应数据来进行运动学、动力学计算。今天就只有晚上稍微写了一下代码，还搞忘了一些概念，看了会儿书。
今日更新
-
AdjMapMat:根据齐次变换矩阵求出对应的坐标系转换关系下的伴随变换矩阵，该矩阵主要用于运动旋量的坐标转换； -
SE3Twist:根据螺旋轴信息来求解齐次变换矩阵，现在有个问题就是如何准备描述一个螺旋轴？因为之前学习的时候都是直接将这个方法带入了机械臂正运动学中，没有单独进行螺旋轴描述过，所以在测试的时候就有点不知所措，不过目前看来程序是没有问题的；
Debug
- 之前的
RotMatExp函数忘记了将旋转轴化为单位轴，现在是增加了一个判断语句来进行归一化。 - 优化了齐次变换矩阵元素挨次赋值的循环语句，将一些语句放在了外侧循环中，减少了语句的循环次数，虽然这是一个尝试，不过也是之前没有注意到的细节啊！
睡觉。
今日更新
GetTwist:由齐次变换矩阵求出其对应的螺旋轴GetTwistTheta:解出螺旋轴对应的旋转角度
今天完成的都是一些非常简单的功能，不过已经大致把刚体运动的部分写完了，接下来就是写机器人运动学的相关内容了，目前正在做的是一个建立机器人DH模型的类RobotDH，但是现在这个类有重大bug，且会干扰到SE3Twist，使其解算结果莫名其妙的错误，让人焦头烂额。
和一个师兄交流后，他指出了我的代码中很多不合理的地方，比如构造函数太复杂了、变量在类里只声明不要定义，我也觉得我的构造函数过于复杂了，可能会引起一些莫名其妙的问题，影响到了SE3Twist，现在这俩还不能一起使用，我接下来打算先去看看别人的代码，学习一下规范构造这些，然后将这个机器人类优化一下，再测试测试。
看代码去咯。
还是没解决
尝试解决昨天使用RobotDH类时，会干扰了SE3Twist的问题。该问题的现象就是：使用这个类时，根据类声明的位置的不同，会使得SE3Twist解出来的矩阵数值不同，起先我怀疑是不是使用了动态大小的Eigen库函数导致的，将类中的所有成员注释后，修改了SE3Twist中使用了动态大小的地方后，问题得到了暂时的解决，但是当我取消掉类成员的注释后，问题又出现了！
在查阅了一些资料后发现，Eigen库的内存对齐问题一直受人诟病，内存对齐这一块我也不是很懂，今天就再去看看相关资料吧。然后在eigen的文档网站发现在类中使用Eigen的方法。如果要在类里面调用Eigen的函数，则必须重载 operator new，以便它生成16字节对齐的指针，只需要在类的public声明中加上一句宏定义EIGEN_MAKE_ALIGNED_OPERATOR_NEW，然后就可以在类中使用eigen的函数来定义变量了。但是该方法是针对静态大小，对我的问题也不适用。
然后晚上无意中在StackOverflow中发现了一个关于<<的重载问题，然后我就发现我的代码出问题的地方就涉及到了好几个<<，我就怀疑可能是迷之重载出了问题，于是就把构造函数中给向量赋值用的<<全部删除，换为挨个赋值，他妈的有时候可以有时候又不行了？调不来了，休息了
心态炸裂，昨天那个机器人类，如果我把所有的成员属性在构造函数里进行赋值，那个问题又出现了！
晚上成功解决该bug，被自己蠢哭了
没错，这个神秘bug解决了……我之前gdb调试的时候，一直没有进入if语句，我还以为是我用next的操作不对，今晚无意中发现，马勒戈壁，if的条件写错了！！！！！！！！怪不得SE3Twist的输出值经常是随机数！因为没有赋值！把if的条件修改后，成功解决该bug，RobotDH类是无辜的，错怪它了，唉！被自己蠢哭了，不过成功debug，还是挺爽的。
在本次Debug的过程中，通过查询Eigen文档、百度、StackOverflow，学到了以下知识：
- 内存对齐：计算机读取数据时，并不是一个字节一个字节的读，而是直接读一大片内存，如果不使用内存对齐，比如一个４字节的变量，前３字节在第一次读的那一片内存里，此时需要舍弃第一个字节，后１字节在另一片内存，需要再读一次，然后舍弃三个字节，就很麻烦，所以将各个类型的变量进行内存对齐，计算机就可以一次性读取数据了
- GDB调试：
s是进入函数内部，n是直接执行完这个函数，-tui或者Ctrl + X + A可以启动图形化界面，方便知道当前执行到哪了 - Eigen作为函数形参，进行值传递时，官网建议使用
const的形式来进行值传递，我已经将之前的函数进行了修改 - 变量的声明和定义写近一点，声明矩阵变量后，注意定义的时候是否有没有赋值的元素？建议矩阵在声明的时候直接定义成0矩阵
- 遇到问题不要慌着认为是库的问题，检查一下自己的语法和逻辑
除此之外，今天还学习了怎么在markdown的readme中，增加图标功能，我在本文档的开头增加了语言和更新日期的图标，非常简单，需要使用Shield.io网站，来生产一个代码：
其中，badge后面就是标签的头、尾、颜色了，非常简单。还可以利用Simple Icons网站来获得图标名字，直接增加一个logo项，即可增加图标了
MakeFile
简单写了一下Makefile，直接在其路径下使用make命令即可自动运行Makefile，自动编译，对于没有修改的文件还可以不重复编译，比我之前直接g++ -o xxxxxxxxxx快了一些，方便得多了，以后增加了文件只需要修改一下Makefile即可，如果要增加GDB调试，则需要在可执行文件链接的那一句在文件名之后再加上-g
bin/robot: obj/main.o obj/BodyMotion.o obj/RobotBuild.o
g++ -o bin/robot -g obj/main.o obj/BodyMotion.o obj/RobotBuild.o
obj/main.o: src/main.cpp src/BodyMotion.hpp src/RobotBuild.hpp
g++ -c -g src/main.cpp -o obj/main.o
obj/BodyMotion.o: src/BodyMotion.cpp src/BodyMotion.hpp
g++ -c -g src/BodyMotion.cpp -o obj/BodyMotion.o
obj/RobotBuild.o: src/RobotBuild.cpp src/BodyMotion.hpp src/RobotBuild.hpp
g++ -c -g src/RobotBuild.cpp -o obj/RobotBuild.o
周末去重庆找女朋友玩了。今天上午把以前的笔记本拿去修了，下午做实验，晚上稍微写了点。
今日更新
RobotTwist类，通过旋量来构建机器人模型，该方法比DH参数法更优越；RobotTwist::FKTwist函数，该函数是在旋量机器人模型类中的成员函数，可以用来计算基于指数积模型的正运动学，目前验证是正确的；- 修正了之前代码无用的
static声明，因为我不会使用局部变量地址、引用来作为返回值，故不需要。
更新的有点少，精力有限，慢慢来吧。
学习总结
- Eigen库的可变大小类，如
VectorXf，在赋值之前，需要对其大小进行声明，否则容易出错； - 对
Vector类进行截取时，不能像Matlab那样直接V(i, j)，而是要使用segment函数omega.segment(3*(DoF - 1 - i), 3)截取omega的3*(DoF-1-i)开始的３个元素。
今天还是只有晚上写了代码。今天复习了一下机器人雅克比矩阵的含义，结合旋量法，对雅克比矩阵有了更深的理解。雅克比可以分为：
- 空间雅克比，其每一列都是对应的关节的旋量在基座标系下的表示，其含义为每个关节转动速度对末端执行器速度的贡献量；
- 物体雅克比，和空间雅克比类似，只不过它的每一列都是在末端坐标系中表示，且顺序有所不同，详见Modern Robotics.
今日更新
RobotTwist::FKBody()：将旋量转为在末端坐标系中，然后求正运动学，结果与基座标系的一致；RobotTwist::JacobianBody()：求机械臂的物体雅克比矩阵；RobotTwist::JacobianSpace()：求机械臂的空间雅克比矩阵。
Debug
- 对
Vector类进行叉乘运算时，必须是静态大小为３的向量才能调用cross方法，哪怕是截取的三维向量都不行，此时需要通过Skew求其叉乘矩阵，再进行乘法即可； - 动态向量不能使用
Zero()进行赋值，该方法只能用于静态向量。
对了，这几天还把我的旧笔记本拿去修理了，我想把它配置成一个Linux服务器，然后搞一些折腾自己的*操作，顺便学习一下服务器、计网……希望一切顺利吧。
啊这，昨天没来得及写，因为服务器上一直没clone下来这个库😂
出于安全和需求的考虑，我还是买了腾讯云的服务器来玩。费了九牛二虎之力，踩了好几个坑，终于用VSCode远程连接上了，并配置好了我的这个库的开发环境，可以在Windows下进行开发咯。详细信息在我的博客可看远程连接配置
踩坑实况
- SSH，因为我之前对SSH不是很了解，所以在配置以SSH远程连接的时候，一脸懵逼，后来才知道了，同一个文件夹中可以放多个不同名的SSH公钥私钥，然后公钥是给远程端的，私钥是放在本地，远程端连接时拿来验证的，所以需要知道私钥的绝对路径；
- config，在写连接的config文件时，我对其user这一项也很懵逼，后来经过我的测试，user这一项就是填写你的服务器里的用户名，root好像不行，所以我就用了服务器默认的lighthouse；
- eigen，在新环境中安装eigen库，我使用
sudo apt install libeigen3-dev，此时它会将Eigen安装到一个/usr/include/eigen3文件夹下，如果直接使用#include <Eigen>则会报错，需要改为#include <eigen3/Eigen>，但是这样的话就需要改很多头文件引用，所以直接将Eigen复制到include文件夹中，sudo cp -r Eigen ../Eigen
接下来就是继续在服务器上写该机器人库咯。
准备开始写逆运动学，但是对旋量法求逆运动学还有点理论上的疑问，所以今天晚上看了会儿书，豁然开朗。
虽然都是使用牛顿迭代法来求一个解析解，但是旋量法的变量比我毕业设计用的那个方法少一半！
旋量法的主要**就是将末端位姿与给定位姿的误差，转为一个运动旋量，这一点我在书上有详细的笔记。通过不断的修改当前末端位姿，使得给定位姿与末端位姿之间的转换矩阵越来越接近于0，因为每个转换矩阵都可以用一个运动旋量来表示，所以这个问题也被转换成了一个六维向量：末端坐标系中的运动旋量不断趋近于0的问题，所以还要使用末端坐标系下表示的雅可比矩阵。
原理已经弄清楚了，明天就开始写！
今日更新
- 将正运动学、微分运动学单独写到一个新的头文件和源文件中：
Kinematics - 将正运动学、微分运动学移除出机器人类
今天写了个逆运动学的雏形，理论上是没有问题的，但是还是有bug，会出现nan的结果，根据我调试时看到的情况，进过迭代后，末端位姿与目标位姿之间的差值其实已经很小了，但是解算成旋量时出现了一些问题，现在准备从我的结算旋量算法开始入手debug。
今日更新
VectorXf IKNewton(RobotTwist &robot, const Matrix4f target, VectorXf initAng)：通过输入机器人对象、目标矩阵、初始角度，即可开始通过牛顿迭代法进行求逆，现在还有bug
Debug
- 是否是一个小数精度的问题？在matlab中，是默认保存4位小数，所以非常方便，但是C++中则有很多位，比如我的一个旋转矩阵为
{1, 5.51380253e-05, 1.37014342e-06, -5.51380035e-05, 1.00000012, -1.44393043e-05, -1.37093969e-06, 1.44392261e-05, 1.00000012}，理论上这个矩阵是一个单位阵了，但是此时因为精度问题，其不仅不是一个单位阵，还出现了迹tr>3的情况，这样就会导致后面使用acos函数出错 - 后来发现好像与这个没啥关系哦，自动给省略了，后面多出来的几位其实就是浮点数二进制表示的误差
今天的精力都用在了逆运动学debug上，翻来覆去看书，看自己的算法是否有问题，顺便搞清楚了运动旋量和螺旋轴的关系，以及其在空间坐标系和物体坐标系下表达的含义。
- 运动旋量为单位时间内，物体的运动速度信息，因此其角速度不需要单位化。而螺旋轴为单位速度，其角速度为一个单位向量，只包含方向信息，不包含角速度大小信息。将螺旋轴乘上旋转角度，即以单位时间内转动的角度，即可得到运动旋量，因此：$V=S\dot{\theta}$；
- 对于角速度，是一个三维向量，$\omega_s$表示其在空间坐标系中的表示，$\omega_b$表示其在物体坐标系中的表示；
- 对于线速度，则需要一个参考点了，因此$v_s$是空间坐标系原点的线速度，$v_b$则是物体坐标系原点的线速度，这个线速度则是由旋转引起的；
- 物体雅可比，是将每个关节的旋量表示在物体坐标系下，然后将每个关节的旋量依次作为雅可比矩阵的列，表示了每个关节对物体坐标系的运动旋量的分量贡献，当两列一样时，即两个关节角的贡献同样的运动旋量，这样就会让机械臂损失一个自由度，雅可比矩阵奇异，自由度退化；
- 空间雅可比与物体雅可比类似，只不过其是在空间坐标系中表示关节旋量，表示的是每个关节对空间坐标系的运动旋量的分量贡献；
- 雅可比与正运动学求法类似，都是求关节旋量，但是正运动学是在关节角为初始角的情况下求出关节旋量，然后通过修改指数积的$e^{S\theta}$中的角度值来修改末端位姿，其运动旋量是不会改变的；而雅可比则是动态的计算运动旋量。
今日更新
- 增加了PUMA560型机械臂的旋量模型，方便将我的算法与Matlab中的结果进行对比、排错；
- 更正了几个算法错误，例如少写了负号、忘记单位化的问题。
Debug
我发现问题所在为在进行物体雅可比求伪逆
会出现无解nan的情况。通过与Matlab运算结果对比，排除雅可比算法、正运动学算法错误。
后来发现我的机械臂的初始角度为$(0.1, 0.1, 0.1, 0, 0, 0)$，是处于一个奇异点位形，目标姿态也是！此时四轴和六轴轴线重合，两者对末端运动旋量的贡献一样，此时雅可比矩阵退化，丢失一个自由度，发生奇异！
修改目标矩阵为非奇异位形，设置一个合适的初始角度，成功求得逆运动学解！算法正确！
这也让我对机械臂奇异位形有了更深的理解，在奇异位形下，雅可比奇异，有多个关节贡献同样的末端运动旋量分量，这就导致了某些关节无法求解。
白天出去玩了，或许这就是周末吧，他妈的晚上开组会开到十点我是真没想到，今天又荒废了，不过还好，学会了一个github pull加速的*操作，非常简单：
- 利用其镜像网站
github.com.cnpmjs.org，替换将要pull的地址中的github.com，即可实现加速
于是今晚我就pull了一个hexo的yilia主题，
sudo git clone https://github.com.cnpmjs.org/litten/hexo-theme-yilia.git yilia
今晚就配置一下我hexo吧！
今天啥都没干，唉。唯一的进展就是把个人博客搭好了，直接往里面写文章就行了。然后在RobotTwist和RobotTwistInit中增加了机械臂的动力学参数，目前只添加了PUMA560机械臂的动力学参数，准备写动力学求解了。
明天还要去卡诺普公司交流，希望能够针对工业机器人面对的真实困难进行研究，不想再只纸上谈兵了，想做点真实的。
更新将会越来越慢，因为最近项目压力有点大，昨天因为事情有点多，晚上双十一逛淘宝去了，没有更新。
11月9日去卡诺普自动化交流之后得知，机械臂的六轴不同步问题，影响了他们的机械臂轨迹精度，也许我最近的项目就将围绕着这个问题进行建模仿真分析，还要去做实验，可能这个库的更新就会停止，不过周末应该还是会更新的。
最近也不是没有收获，跟学机械的同学交流后，我对转动惯量有了更深的理解，也方便我去编写动力学解算代码。
- 转动惯量就是描述一个物体，绕某个轴旋转时的性质的，类似于$F=ma$中的质量，绕不同的轴旋转，会有不同的转动惯量表现在这个轴上，转动惯量越大，转动越困难，即同样的力矩，旋转加速度越慢；
- 惯量主轴存在于一个“质量对称”的位置，绕该轴旋转的转动惯量只有主对角线有元素。质心位置必定有惯量主轴（待我仿真验证），可以通过在质心处建立任意旋转轴，求出转动惯量，求出其特征值、特征向量，将该坐标轴进行转换，应该就可以得到惯量主轴**（待验证）**
代码可能不会有什么突飞猛进，因为我对动力学RNE（递推牛顿欧拉法）还不是特别熟悉，而且最近做项目的兴趣也比较大，暂停一下吧！
嗯，最近感觉这个库也没啥好写的了，仔细一想，机器人库似乎都是一些比较底层的逻辑，对业务要求几乎为0，那要不要试试写个C语言版本的？说干就干，但是要先整个C语言矩阵运算库出来，这段时间就准备写这个了，顺便再复习一下C语言，此库##暂停更新##。