[双足机器人]pybullet单足仿真-解决机器人中存在闭链机构的问题

项目背景：

项目要做一个双足机器人，先做一条腿的仿真，验证电机的输出力矩是否能够实现跳跃功能。

在做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。

我把我机器人中对应的关节也做了同样的修改

卧槽！！皇天不负有心人

我终于成功了

牛批！牛批