V-rep学习笔记：机器人模型创建3—搭建动力学模型

　　接着之前写的V-rep学习笔记：机器人模型创建2—添加关节继续机器人创建流程。如果已经添加好关节，那么就可以进入流程的最后一步：搭建层次结构模型和模型定义（build the model hierarchy and finish the model definition）。但是想要进行动力学仿真（碰撞、接触、自由落体...）的话，还需要进行额外的一些操作：

• Building the dynamic shapes

　　VREP中几何体的属性可以分为：

1. dynamic or static: 具有动态属性的物体会在重力影响下坠落或在其它外力/力矩作用下发生运动状态的改变。而静态物体则会在场景中静止不动，如地面，或跟随其父节点运动（or follow the movement of its parent in the scene hierarchy）。
2. respondable or non-respondable:  respond是回答，响应的意思。a respondable shape will cause a collision reaction with other respondable shapes. 即respondable属性用于控制物体的碰撞响应，non-respondable物体在与其它物体发生碰撞时，物理引擎不会进行计算，会出现穿透的现象。

　　During dynamic simulation, static shapes will not be influenced (i.e. their position relative to their parent object is fixed), whereas non-static shapes will be directly influenced by gravity or other constraints (e.g. dynamically enabled joints. Respondable shapes influence each other during dynamic collision (i.e. they produce a mutual collision reaction, they will bounce off each other).

　　可以进行动力学仿真的物体在仿真过程中可以通过下面的图标来识别：

[Icon marking dynamically simulated objects]

　　当由于某些原因导致物体不能进行动力学仿真时，会提示警告：

[Warning icon when an object cannot be dynamically simulated]

　　 为了进行快速稳定的仿真计算，Respondable物体的形状应该越简单越好。物理引擎可以仿真下面5种几何体，其性能和稳定性各有差异：　　

　　1. Pure shapes: a pure shape will be stable and handled very efficiently by the physics engine. The draw-back is that pure shapes are limited in geometry: mostly cuboids, cylinders and spheres. If possible, use those for items that are in contact with other items for a longer time (e.g. the feet of a humanoid robot, the base of a serial manipulator, the fingers of a gripper, etc.).

　　看下面的一个例子，左边的立方体是在Blender软件中导出的STL网格模型，是一个最简单的8个顶点的立方体；右边是VREP中添加的Primitive立方体。将它们设为动态物体，并且respondable，放置在地面上。选择Bullet物理引擎开始仿真，可以看出左边导入的网格模型不稳定，一会就掉了下去，而右边的primitive shape始终保持稳定；如果物理引擎换成ODE，开始仿真一会左边导入的模型将会弹飞...；如果换成Vortex或Newton引擎，结果会好很多，两个物体都能稳定在地面。因此，机器人模型与外界环境发生交互和接触的部分最好是Pure shape，以提高计算效率和稳定性。At any time, the user is free to quickly switch from one engine to the other according to his/her simulation needs. The reason for this diversity in physics engine support is that physics simulation is a complex task, that can be achieved with various degrees of precision, speed, or with support of diverse features.

　　2. Pure compound shapes: a pure compound shape is a grouping of several pure shapes. It performs almost as well as pure shapes and shares similar properties. Pure compound shapes can be generated by grouping several pure shapes.

　　在VREP软件的motionPlanningAndGraspingDemo.ttt场景中桌子上有一个水杯，这个水杯能稳定地与桌面接触。虽然看上去这像是一个导入的网格体，但实际上它是由许多pure cuboid组成的pure compound shapes（见图中右侧的杯子），它被隐藏在图层9中，只用网格模型来做显示，而用更稳定的pure compound shapes来计算与桌面的接触。

　　3. Convex shapes: a convex shape will be a little bit less stable and take a little bit more computation time when handled by the physics engine. It allows for a more general geometry (only requirement: it need to be convex) than pure shapes.

　　Whenever possible, try using pure shapes as respondable shapes. This is however not always easy to do, or practical. In that case, you can generate a convex shape, or a convex decomposed shape. Convex shapes perform faster and are more stable than random shapes (but they are still not as fast and stable as pure shapes!). Select the shapes you wish to simplify as convex shapes, then select [Menu bar --> Edit --> Morph selected shapes into convex shapes...]

[Non-convex model (left) and corresponding convex-decomposed model (right)]

　　4. Compound convex shapes: a convex compound shape is a grouping of several convex shapes. It performs almost as well as convex shapes and shares similar properties. It can be generated by grouping several convex shapes.

　　5. Random shapes: a random shape is a shape that is not convex nor pure. It generally has poor performance (calculation speed and stability). Avoid using random shapes as much as possible.

　　注意当直接把简化后的网格体设为respondable时会弹出警告对话框，提示这样做会影响物理引擎的计算性能，最好使用pure shapes或convex shapes代替。

　　只从性能来看，选择的优先顺序为：pure shapes > pure compound shapes > convex shapes > compound conves shapes ≫ random shapes. 综合仿真性能以及几何外观，我们考虑使用基本圆柱体作为UR机器人基座，凸壳或其组合作为连杆来构建机器人动力学模型。用圆柱体和凸壳搭建机器人动态模型虽然可以提高计算性能，但是如果直接用它作为外观展示就不太好了，这样会显得模型比较粗糙。因此，我们可以将其移动到隐藏图层中，作为之前创建的可视化模型的对应。The hidden part will represent the dynamic model and be exclusively used by the physics engine, while the visible part will be used for visualization, but also for minimum distance calculations, proximity sensor detections, etc.

　　下面以官方帮助文档中的UR5机器人为例进行讲解。首先处理机器人基座：进入网格编辑模式，先删除多余的线缆，然后选中所有面后提取圆柱体，如下图所示：

[Pure cylinder generation procedure, in the triangle edit mode]

　　将生成的圆柱体重命名为robot_dyn，并移动到图层9中隐藏起来。剩下的连杆可以用凸壳或凸壳组合来模拟。先选中第一个连杆，然后通过[Menu bar --> Add --> Convex hull of selection]来添加凸壳，同样将其重命名为robot_link_dyn1并移动到图层9中隐藏。

[Original shape, and convex shape]

　　当提取的凸壳包含的形状细节不够多时可以先手动提取多个凸壳，然后将其合并为一个整体[Menu bar --> Edit --> Grouping/Merging --> Group selected shapes]；或者可以使用VREP提供的自动提取凸壳组合体的工具[Menu bar --> Add --> Convex decomposition of selection]：

　　这里用到了一个开源库V-HACD（The V-HACD library decomposes a 3D surface into a set of "near" convex parts），这个开源库可以将原始形状复杂的几何体提取成凸壳组合，既保留了凸壳的运算性能，又逼近了原始形状。

　　至于为什么有时不能单纯的用凸壳来代替原始物体可以看看下面这个例子。下图中原本加上尾翼的汽车尾部是呈现凹状的，但由于使用凸壳来近似代，如果放一个箱子在那个位置，会导致箱子“悬浮”在汽车上面，违反物理常识。一个更直观的例子是如果我们把一个碗简单的提取出凸壳，那么我们将不能在这个碗里放入任何东西！Objects will just float on top of it. In this case, we can use a convex decomposition of the concave shape.

　　鉴于上面这种情况，就需要将凹状物体（或形状复杂的物体）分解为几个简单凸壳的组合。在许多例子中，凸分解（convex decomposition）的工作常常由手工完成，因为实时仿真中最重要的就是性能，而几何体的形状会严重影响计算性能（It’s generally a good idea to create very simple physical representations for complex graphic objects）。下图是凸分解后的汽车：

　　再回到我们的UR机器人，从下图可以看出与之前简单添加的凸壳相比使用V-HACD提取的凸壳组合更接近真实形状：

[Original shape, and convex decomposed shape pendant]

　　接下来我们可以使用同样的方法处理剩下的几个连杆，处理完成后将每个表现外观的几何体拖到其相应的动态几何体下（attach each visible shape to its corresponding invisible dynamic pendant） ：

[Visible shapes attached to their dynamic pendants]

　　到这里我们的动力学模型基本已经构建完毕，但还有下面一些选项要设置：

　　首先，我们搭建在隐藏图层的动力学模型只是为了供物理引擎进行动力学计算，因此其它一些不必要的计算可以取消。在物体的Object common properties属性对话框的Object special properties栏去掉所有选项

　　然后，设置每个连杆的碰撞属性: 勾选Body is respondable选项。并勾选基座的Local respondable mask的前4位，连杆1相应的后4位，连杆2前4位...这样一直交替错开，避免机械臂自身连杆之间的碰撞相应（once the model will be defined, consecutive dynamic shapes of the robot will not generate any collision response when interacting with each other）。

　　Respondable mask: indicates when a collision response is generated. The mask is composed by two 8-bit values, local and global. If two colliding shapes share any of their parents (direct or indirect), then the local masks are used, otherwise the global masks are used. If two shapes' AND-combined masks (local or global) is different from zero, then a collision response will be generated. 即当两个物体碰撞掩码按位与时如果得到非零值，则两个物体干涉时会发生碰撞；如果得到零，则两个物体会相互穿透。

　　下面举一个简单的例子：创建两个立方体（没有共同的父节点），则Global respondable mask会起作用。第一个立方体勾选前面4位，第二个立方体勾选后4位，掩码之间没有重合，因此按位与的结果为0。仿真时上方的物体自由坠落，会穿透下面的立方体，最终落在地板上。如果将上面的立方体掩码全部去掉，仿真时它会连地板一起穿透，直接掉下去（因为去掉了Global respondable mask的所有位，不会与场景中任何物体发生碰撞）。

　　最后，将之前提取的凸壳设置为dynamic：在Rigid Body Dynamics Properties对话框中勾选Body is dynamic。我们可以人工输入几何体的质量和惯性矩，或者通过点击"Compute mass & inertia properties for selected convex shapes"按钮，根据几何体形状和设定的密度自动计算出质量以及惯性矩。

　　注意机器人的基座有些特别：通常基座都固定在地面静止不动，因此不需要设置成动态体，否则仿真时会掉下去。但当我们想把机器人装在一个移动小车上使用时，就要将其设置为dynamic。在动力学属性对话框中先勾选"Set to dynamic if gets parent"，然后去掉 "Body is dynamic"选项，即可实现把基座装在移动小车上时它是动态的，单独使用（没有父节点）时它是静态的。

参考：

Building a clean model tutorial

Designing dynamic simulations

Khaled Mamou's Blog

https://github.com/kmammou/v-hacd

Video Game Physics Tutorial - Part II: Collision Detection for Solid Objects