qq机器人源码_机器人动力学求解库使用笔记

最近笔者因为项目的原因需要求解自己的腿足机器人的全身动力学模型，腿足机器人因为身体（基座）是在不停运动的，因此其动力学模型是浮动基动力学。笔者的理解是浮动基模型其实就是在机器人身体与世界坐标系原点之间假想了一个6自由度的关节（3旋转+3平移），然后按照基座固定的工业机器人的动力学求解方案求解即可。

MIT开源的cheetah程序中的floating base model是他们自己手写的，相关的理论可在《Rigid Body Dynamics Algorithms》（https://www.springer.com/us/book/9780387743141）中查询，笔者不是专门研究动力学的，因此只是查找一些合适的动力学求解库。不得不说，感觉动力学求解库这块的资料蛮少的，虽然库有很多，但基本都不怎么友好，上手难度大，因此这里记录下笔者尝试过的几个动力学求解库。

笔者最近共尝试了pinocchio，frost，drake这三个动力学求解库，三者都是基于URDF模型计算动力学。

pinocchio：

是一个法国团队开发的，据笔者了解目前用的实验室蛮多的，支持C++以及Python，不支持符号运算，每次运算直接把具体数值传进去，然后取出M,C,G矩阵的具体数值，支持的平台如下：

因为笔者用的是qnx系统开发机器人，基于跨平台移植工作量的原因就没考虑它了，关于它的使用资料可以看一下他们团队今年的一个线上课程，有讲稿和示例代码：https://memory-of-motion.github.io/summer-school/，示例代码可以跑通。

frost：

Cassie的动力学就是用它算的，支持matlab，支持符号运算，可以直接求出动力学模型的符号表达式，软件安装教程详见：https://ayonga.github.io/frost-dev/pages/installation.html，按照这个步骤安装就可以了。这个库很友好的一点是Cassie的动力学求解代码完全可以当作一个很好的示例，https://github.com/UMich-BipedLab/Cassie_Model，对于自己的机器人，只要修改urdf模型和cassie.m中的一些关节指定代码，就可以开始计算自己的动力学了。

备注：过程中可能出现的问题：

1. 跑cassie示例代码时会报错生成的中间文件编译失败，这个感觉是个玄学问题。笔者当时在笔记本上每次都是这个报错了，后来在台式机一次性成功了，台式机软件版本：matlab 2019b + mathamatica 12.1，仅供参考
2. 换成自己的urdf模型后ros_load_urdf（）函数会报错，matlab显示什么不能用点索引啥的，这个其实就是你的urdf模型不规范，建议对照cassie的urdf写，或者直接根据报错行数看一下自己urdf是哪个标签出了问题

drake：

drake是MIT的“underactuated robotics”课程的配套软件，这门课程的授课老师是Russ Tedrake教授，就是2015年MIT参加DRC时的领队，这门课程也很经典，视频可在youtube观看：https://www.youtube.com/channel/UChfUOAhz7ynELF-s_1LPpWg/videos，讲稿可以直接访问：http://underactuated.mit.edu/，全程干货满满，课堂上的示例代码也都是开源的：https://github.com/RussTedrake/underactuated。

drake，支持C++，Python，支持符号运算，要计算自己的动力学模型可在示例代码double_pendulum的基础上替换urdf模型就可以了，很方便。支持的平台：macos , ubuntu(18或者20)，安装的话推荐使用二进制文件安装，编译源码安装解决依赖很头疼：http://underactuated.mit.edu/drake.html

笔者是在underactuated/examples/double_pendulum/dynamics.ipynb上稍微改动了一下，ipynb就是一种可标记的python语言，安装jupyter notebook即可打开，修改后的代码如下：

注意drake算的浮动基模型的前六个位置量分别是四元数（wxyz）和位置（xyz），所以位置维度会比速度维度大1，当然动力学矩阵维度是按照速度维度的。这个代码是笔者使用了cassie的urdf模型。

随便记录一下，错误不严谨之处，还请不吝赐教，笔者QQ: 1876421464