项目背景：
项目要做一个双足机器人，先做一条腿的仿真，验证电机的输出力矩是否能够实现跳跃功能。
在做pybullet的仿真之前，采用的是gazebo进行仿真，但是由于gazebo中urdf模型文件不支持闭链机构，sdf文件虽然支持闭链机构，但是只能通过编写插件的方式进行关节的控制。
看到pybullet的样例中四足机器人minitaur中存在闭链机构，并且能够实现关节的控制，就决定使用pybullet进行仿真。
解决过程：
之前虽然看过pybullet的一些仿真项目的代码，但是没有真正的自己动手写过程序。
找到pybullet官方的使用文档，看了一下get start的代码Hello Pybullet World
我内心窃喜，这可难不倒我ctrl-CV大法。
通过简单的分析代码，大概知道先加载了地面，然后加载了机器人，再进入仿真循环
知道了大概流程，就开始我们自己的手术实验。
第一次-加载sdf文件
既然要仿真自己的机器人模型，那就要加载自己的机器人模型。看到pybullet中有loadSDF函数，心中窃喜，这不是白给。gazebo中早就吧sdf文件准备好了。
robot = p.loadSDF("/home/craipy/PycharmProjects/single_leg_simulation/model/single_leg_v7/model copy.sdf")
？？怎么回事？？
怎么报错了，我sdf文件gazebo中是能够加载的啊
尝试把sdf中的闭链关节去掉
啊？
掏出pybullet说明文档
为什么？pybullet演我？要你有何用
第二次-构建约束
不行，掏出我的百度大法
在知乎中看到了一个小伙伴和我遇到了同样的问题
但遗憾的是这位朋友最后问题并没有得到解决。
然后我查阅了一下pybullet的说明文档
首先createContraint方法，需要提供链接这个约束点的两个linkId，但是我在pybullet的说明文档中怎么也没有找到关于如何能够获取linkId的方法。
通过getNumJoints方法与getJointInfo方法能够获得每个Joint的id与这joint的父Linkid，我进行了一步推测，由于构建模型文件的时候，每一个joint对应着一个子link，子link的id应该与jointid是一样的。（后面阅读minitaur的源码文件，发现是这样的道理）
勉强解决了2个参数，还有两个很重要的参数，parentFramePosition与childFramePosition。这两个参数给我整的迷迷糊糊的。为了定位到约束点，需要确定约束点在父Link中的位置与在子link中的位置。
这可不好整了，我根本就不知道link的Position和orientation怎么确定这两个参数呢？
继续在说明文档找说明
这让我明白了pybullet包含joint坐标系，和link的质心坐标系。
我又不断的尝试，试图找到link的质心坐标。在urdf文件生成过程中，我明白了urdf文件中 inertial->origin是决定link的质心坐标系的关键。但具体是怎么决定的还需要进一步尝试。
<inertial>
<origin xyz="-1.49612837680224E-16 0.0680546106444214 0.450099514805464" rpy="0 0 0" />
<mass value="7.53402464620325" />
<inertia ixx="2.04472009475898" ixy="9.13747033457407E-16" ixz="-3.39571306621468E-16" iyy="2.14218400370622" iyz="0.111298129150483" izz="0.24400607242942" />
</inertial>
我在solidworks中把每个关节的坐标点和每个link的质心都以绝对位置的形式记录下来。
在将urdf文件中的joint的origin与link的origin记录下来。
很快就发现了其中的规律
joint的origin是相对前一个joint的origin建立的。
link的origin是相对joint的origin建立的
知道这个建系规则，就能够确定指定link的质心坐标，进而确定约束点在质心坐标系中的位置。
然后我发现有一个getLinkState的方法，能够直接获得制定link的质心坐标系
知道这些，我想这回应该能成功了吧！！！
然后
？？？为什么
人最迷茫的时候不是遇到困难，而是遇到困难没有方向解决。
我怀疑是我的坐标位置弄错了，但是我绘图出来是没有问题的，但是就是无法形成约束关系。驱动关节的位置一改变，约束关节就裂开了。
我人也裂开了。。
我尝试找到minitaur的源码，看看他是怎么添加约束的，于是下载了bullet的仓库，这里面有minitaur的例程
我还是觉得是我把关节位置弄错了
于是在minitaur机器人用同样的思路把约束点画出来，发现没有错。
说明约束点的位置没有错。
对比其他的例子，我发现一个共同点，minitaur在创建了约束点后，会将不是驱动关节的控制方式修改为VELOCITY_CONTROL。
我把我机器人中对应的关节也做了同样的修改
卧槽！！皇天不负有心人
我终于成功了
牛批！牛批
