接触力的计算
游戏或者仿真中要提供接近于真实世界的完整触觉反馈体验，需要VR头戴设备、控制器、外骨骼甚至是行走模拟装置的配合。然而，人类的触觉系统极其敏感，普通人打麻将就能用手指轻轻松松地摸出牌面。在目前的技术基础上，机器很难还原真实的人类触觉反馈，能做的只是在特定的内容和场景中尽量满足用户的反馈体验。比如在VR游戏中抓取武器和物品时，几厘米的偏差，没有准确还原物体材质和纹路，对用户的实际体验影响并不大。除了高精度的外设，要实现触觉或力反馈，还需要强大的物理引擎，虚拟世界中的复杂物体的建模、与用户肢体的触碰非常消耗计算资源。
- 牛顿运动定律与物理引擎
- 惯性定律：如果物体没有受到外力作用，则运动状态（静止或匀速直线运动）不会发生改变；
- 牛顿第二定律：物体的加速度跟其所受合外力成正比，跟物体质量成反比，加速度方向与合外力方向相同，即$F=ma$；
- 牛顿第三定律：力是物体与物体之间的相互作用，作用力与反作用力总是大小相等方向相反，作用在同一直线上。
根据牛顿三大运动定律，我们可以创造物理引擎来使虚拟场景中的物体产生动态行为，提供沉浸式体验。对于一个交互式游戏来说其基本运行流程如下：
计算物体间的碰撞是物理引擎最核心的部分。一般来说，先要解决如何高效地检测到碰撞的产生（碰撞检测），以及如何确定碰撞点及方向，之后我们就可以求得碰撞体的受力情况，从而根据牛顿运动定律计算出它们将要产生的平动和转动。最后将场景中物体的位置和姿态输出给图形引擎去渲染。
但如何求得力和力矩？这便是一个很复杂的问题了。像重力可以直接影响物体所受力，摩擦可以直接影响物体所受力矩，这些都很简单。较为复杂的就是两个物体间的碰撞了，物体引擎中有一半以上的代码是用来计算碰撞的。
在两物体接触面上存在着接触面法向力（冲击力或接触力）和接触面切向力（摩擦力）两种作用。当接触不连续时产生的接触法向力就是冲击力。由于摩擦力本身也是很复杂的问题，很多仿真软件中对它作了简化，采用比较简单的库伦摩擦力模型来计算：$$F_f=\mu F_n$$
- 用于碰撞的动力学基本定理
由于碰撞时间极短，通常只有千分之一甚至万分之一秒，因此所产生的力非常巨大。这种产生在碰撞中，作用时间极短，数值巨大的力称为碰撞力瞬时力。瞬时力的冲量称之为碰撞冲量。瞬时力不仅数值巨大，而且随时间迅速变化，其规律非常复杂，难以确定。碰撞过程中除了由碰撞力引起物体塑性变形外，同时还伴随着发声、发光和发热等机械能转换为其它形式能量的现象。因此，在研究碰撞问题时，一般并不去讨论瞬时力本身，而只讨论它的冲量及产生的总效果。研究碰撞问题，各微分形式的动力学基本定理不能直接应用，一般用积分形式的动量定理和动量矩定理。
当两物体碰撞时，过接触点作物体表面的公法线，此法线称为碰撞法线。若两物体的质心均位于碰撞接触点的公法线上，则为对心碰撞；否则为偏心碰撞。
1. 冲量定理
对于由$n$个质点组成的质点系，其中第$i$个质点受到的碰撞冲量可分为外碰撞冲量$\bf I_i^{(e)}$和内碰撞冲量$\bf I_i^{(i)}$，由于内碰撞冲量大小相等方向相反，则质点系在碰撞开始和结束时的动量的改变等于作用于质点系的外碰撞冲量的矢量和：$$ m{\bf u_c} - m \bf v_c=\sum I_i^{(e)}$$
其中，$m$为质点系质量，$\bf v_c$和$\bf u_c$分别为质心碰撞开始和结束时的速度。
2. 冲量矩定理
质点系对质心的动量矩的改变等于外碰撞冲量对质心矩的矢量和：$$ \bf L_{c2}-L_{c1}=\sum M_c (I_i^{(e)})$$
式中，$\bf L_{c1}$和$\bf L_{c2}$分别表示碰撞开始和结束时质点系对质心的动量矩矢；$\sum M_c (I_i^{(e)})$为外碰撞冲量对质心之矩的矢量和。
对于平面运动刚体的碰撞问题，可应用质点系相对于质心的冲量矩定理来描述转动部分。根据刚体平面运动特征，相对于质心的冲量矩定理在其平面内视为代数量，即有$$L_c=J_c \omega$$
式子中$J_c$为刚体对过质心且与其对称平面垂直的轴的转动惯量，$\omega$为刚体转动的角度。由冲量矩定理有：$$L_{c2}-L_{c1}=J_c \omega_2-J_c \omega_1=\sum M_c (I_i^{(e)})$$
式中，$\omega_1$和$\omega_2$分别为平面运动刚体碰撞前、后的角速度。
- 恢复系数
碰撞过程可分为两个阶段。开始碰撞到物体速度为零的过程为第一阶段，称为变形阶段。碰撞冲量为$\bf I_1$，则根据冲量定理：$$0-(-mv)=\it I_1$$
之后，物体恢复弹性变形到碰撞结束的过程为第二阶段，称为变形恢复阶段。设碰撞冲量为$\bf I_2$，则根据冲量定理：$$mu-0=\it I_2$$
经研究发现，对于材料确定的物体发生正碰撞时，碰撞前后的速度大小之比几乎是不变的，等于一个常数$e$，即$$e=\frac{u}{v}=\frac{I_2}{I_1}$$
$e$称为恢复系数。恢复系数$e$表示物体在碰撞前后速度的恢复程度和物体变形的恢复程度，也反映了碰撞中机械能损失的程度。$e$越小，动能损失越大；反之，动能损失越小。一般情况下，$u<v$，恢复系数小于1。但理论上，恢复系数也可以大于1。例如当两个手雷碰撞在一起产生爆炸，化学能转换为机械能（能量增加），碰撞后的速度大于碰撞前速度。另外需要注意：恢复系数是两个碰撞物体之间的共同性质，但在许多文献中恢复系数常写为单个物体固有属性（没有提这物体到底是与哪个物体相互碰撞），在这种情况下，第二个物体被假定为完全弹性刚体。
- ADAMS中的冲击力模型
We can either use information about the depth of the collision and generate a very large force acting on the two objects (like a very stiff spring due to the “compression” of the object’s material as one penetrates the other), or we can use an impulse response which means we simply modify the objects’ momentum without any regard for the forces involved.
ADAMS中计算法向接触力大小有两种模型：冲击函数模型（Impact）和恢复系数模型（Restitution）。恢复系数模型基于冲量理论。冲击函数模型用一个弹簧-阻尼模型来表示（碰撞力由两个部分组成：一个是由于两个构件之间的相互切入而产生的弹性力；另一个是由于相对速度产生的阻尼力），冲击函数表示为：$$F_n=k\cdot g^e+step(g,0,0,d_{max},c_{max}) \cdot\frac{dg}{dt}$$
式中：$g$——接触物体之间的穿透深度；$F_n$——法向接触力大小；$k$——刚度系数；$e$——碰撞系数，反映了材料的非线性程度；$c_{max}$——最大阻尼系数，表示物体碰撞时的能量损失；$d_{max}$——最大切入深度，它决定了何时阻尼力达到最大；为了防止碰撞过程中阻尼力的不连续，式中采用了step函数，其形式为$step(x,x_0,h_0,x_1,h_1)$
[Damping Coefficient versus Penetration]
- VREP中获取接触力的方法
机器人仿真中常常需要测量脚底压力分布，或是手爪夹持力等接触力。在VREP中获取接触力有两种方法：一种是通过在末端添加力传感器进行测量，另一种是直接利用物理引擎计算结果获取接触力信息，相关函数为simGetContactInfo：
使用simGetContactInfo函数可以直接获取接触力信息，不用添加力传感器等物体，但是用法稍微复杂。VREP中默认情况下物理引擎运行速度是仿真速度的10倍，就是说如果仿真一步需要50ms，那么物理引擎已经运算了10步（5ms一步），如果dynamicPass参数设为sim_handle_all，则一步仿真将会返回10次接触力的结果。如果要利用接触力的信息，需要解析这些力属于哪两个接触体。参考官方论坛中的帖子Get Friction between leg-tip and ground：
Is it possible to detect the friction between the tip of a robot-leg and the ground?
You have several ways of doing this:
- you can attach a force sensor at the tip of your leg, then attach a contact sphere to the force sensor (i.e. legTip --> forceSensor --> contactSphere). Then you can read the force/torque in the force/torque sensor
- you can parse all contact points and vectors for one simulation step and try to figure out which one(s) correspond to a specific leg/ground pair. This is more complicated, but you do not require an additional contact sphere as in above's first point. For that, have a look at the demo model Models/other/contact display.ttm (which uses the API function simGetContactInfo)
VREP中提供了一个contact display模型（Model browser --> other文件夹下），利用simGetContactInfo函数方便的为用户显示接触点和接触力的大小以及方向。修改其lua代码，在状态栏中显示每次获取的接触力信息。从图中可以看出立方体与地面接触点有4个，将这些接触点对应的接触力加起来就是立方体重量。
核心代码是下面这句，返回值objectsInContact是相互接触两物体的句柄，contactPt是接触点的三维坐标，forceDirectionAndAmplitude是力向量的三维坐标，幅值代表大小：
objectsInContact,contactPt,forceDirectionAndAmplitude = simGetContactInfo(sim_handle_all, sim_handle_all, index)
仿真时可以看到四个点的接触力数值是一直在跳动的（理论上如果接触面是平面，四个点接触力应该一样），也许是三点确定一个平面，多一个冗余的接触点就会导致计算不稳定？下图中的球与地面只有一个接触点，仿真时的接触力要稳定许多。而且接触力的仿真结果还与具体的物理引擎有关，Bullet和ODE的结果就比Vortex和Newton差很多...
为了得到稳定可靠地接触力，可以对一次仿真中的所有接触力求平均值（参考官方论坛帖子Impact force on collision），下面修改了contact display中的代码，将每一步获取的力（对于单个立方体与地面接触模型，每一步仿真共有40个接触点的接触力）求均值，计算立方体重量：
if (sim_call_type==sim_childscriptcall_initialization) then black={0,0,0} purple={1,0,1} lightBlue={0,1,1} options=0 forceVectorScaling=simGetScriptSimulationParameter(sim_handle_self,'forceVectorScaling') forceVectorWidth=simGetScriptSimulationParameter(sim_handle_self,'forceVectorWidth') contactPointSize=simGetScriptSimulationParameter(sim_handle_self,'contactPointSize') overlayDisplay=simGetScriptSimulationParameter(sim_handle_self,'overlayDisplay') --sim_drawing_overlay: if specified, then items are drawn on top of other objects and are (almost) always visible if (overlayDisplay) then options=options+sim_drawing_overlay end -- Add a line and a sphere container: --sim_drawing_lines:items are pixel-sized lines. 6 values per item (x0,y0,z0,x1,y1,z1) lineContainer = simAddDrawingObject(sim_drawing_lines+options,forceVectorWidth,0,-1,1000,black,black,black,purple) --items are "sphere points". 3 values per item (x,y,z) sphereContainer = simAddDrawingObject(sim_drawing_spherepoints+options,contactPointSize,0,-1,1000,black,black,black,lightBlue) ObjectHandle = simGetObjectHandle("Cuboid") end if (sim_call_type==sim_childscriptcall_cleanup) then -- Remove the containers: simRemoveDrawingObject(lineContainer) simRemoveDrawingObject(sphereContainer) end if (sim_call_type==sim_childscriptcall_sensing) then -- empty the containers: simAddDrawingObjectItem(lineContainer,nil) simAddDrawingObjectItem(sphereContainer,nil) -- Fill the containers with contact information: index = 0 -- zero-based index of the contact to retrieve sum = 0 while (true) do -- Retrieves contact point information of a dynamic simulation pass objectsInContact,contactPt,forceDirectionAndAmplitude = simGetContactInfo(sim_handle_all, ObjectHandle, index) if (objectsInContact) then sum = sum + forceDirectionAndAmplitude[3] if (index == 39) then str = string.format("%f N", sum/10) simAddStatusbarMessage(str) -- Adds a message to the status bar end line={contactPt[1],contactPt[2],contactPt[3], 0,0,0} line[4]=contactPt[1]+forceDirectionAndAmplitude[1]*forceVectorScaling line[5]=contactPt[2]+forceDirectionAndAmplitude[2]*forceVectorScaling line[6]=contactPt[3]+forceDirectionAndAmplitude[3]*forceVectorScaling simAddDrawingObjectItem(lineContainer,line) -- Draw force vector simAddDrawingObjectItem(sphereContainer,{contactPt[1],contactPt[2],contactPt[3]}) -- Draw contact point index = index + 1 else break end end end
代码中还涉及到了脚本仿真参数，通过设置仿真参数可以方便地修改控制脚本中的某些变量，比如线的宽度，接触点大小，比例因子等等。The main script and each child script have a list of simulation parameters. Those parameters can be used as a quick way of adjusting values of a specific model. 在脚本文件中可以通过simGetScriptSimulationParameter函数来获取用户输入的参数。仿真参数图标位于脚本图标右侧，如下图所示分别是没有定义和定义了仿真参数的图标样式：
[Script simulation parameter icons (1) empty parameter list, (2) non-empty parameter list]
双击仿真参数图标可以打开参数设置对话框，点击Add new parameter按钮可以插入新的仿真参数。Value处输入参数的值，Unit为参数单位（这里只起到提示作用）
[Script simulation parameters dialog]
- Parameter is private: if enabled, then the selected parameter is not shown during a simulation (in that case the parameter is probably not meant to be modified during a simulation).
- Parameter is persistent: if enabled, then the selected parameter will not be restored to its original value at simulation end.
获取仿真参数的函数simGetScriptSimulationParameter原型如下：
boolean/number/string parameterValue=simGetScriptSimulationParameter(number scriptHandle,string parameterName,boolean forceStringReturn=false)
函数参数scriptHandle为脚本句柄，或取值sim_handle_main_script或sim_handle_self；参数parameterName为仿真参数名（取值为脚本参数对话框中插入的参数名）；参数forceStringReturn默认为false，该参数用于指定是否将仿真参数值当作字符串。比如，如果该参数设为true，则contactPointSize的值0.01会被函数返回为string类型，即"0.01"，而非number类型。
下面是一个斜面接触的例子，如下图所示质量为1Kg的立方体由于静摩擦力在30°斜面上保持静止，可以看出下方两个接触点的接触力要明显大于上方的两个（不像与水平面接触时那样均匀）。计算四个点接触力矢量的幅值并相加可以求得立方体与斜面接触力的大小，理论上这个力为mgcos30°≈8.66N，实际仿真时输出的结果与理论值一致：
if (sim_call_type==sim_childscriptcall_initialization) then black={0,0,0} purple={1,0,1} lightBlue={0,1,1} options=0 forceVectorScaling=simGetScriptSimulationParameter(sim_handle_self,'forceVectorScaling') forceVectorWidth=simGetScriptSimulationParameter(sim_handle_self,'forceVectorWidth') contactPointSize=simGetScriptSimulationParameter(sim_handle_self,'contactPointSize') overlayDisplay=simGetScriptSimulationParameter(sim_handle_self,'overlayDisplay') if (overlayDisplay) then options=options+sim_drawing_overlay end -- Add a line and a sphere container: lineContainer=simAddDrawingObject(sim_drawing_lines+options,forceVectorWidth,0,-1,1000,black,black,black,purple) sphereContainer=simAddDrawingObject(sim_drawing_spherepoints+options,contactPointSize,0,-1,1000,black,black,black,lightBlue) ObjectHandle = simGetObjectHandle("cube") end if (sim_call_type==sim_childscriptcall_cleanup) then -- Remove the containers: simRemoveDrawingObject(lineContainer) simRemoveDrawingObject(sphereContainer) end if (sim_call_type==sim_childscriptcall_sensing) then -- empty the containers: simAddDrawingObjectItem(lineContainer,nil) simAddDrawingObjectItem(sphereContainer,nil) -- Fill the containers with contact information: index=0 sum = 0 while (true) do objectsInContact,contactPt,forceDirectionAndAmplitude=simGetContactInfo(sim_handle_all,ObjectHandle,index) if (objectsInContact) then sum = sum + math.sqrt(forceDirectionAndAmplitude[1]*forceDirectionAndAmplitude[1]+forceDirectionAndAmplitude[2]*forceDirectionAndAmplitude[2]+forceDirectionAndAmplitude[3]*forceDirectionAndAmplitude[3]) if (index == 39) then str = string.format("%f N", sum/10) simAddStatusbarMessage(str) -- Adds a message to the status bar end line={contactPt[1],contactPt[2],contactPt[3],0,0,0} line[4]=contactPt[1]+forceDirectionAndAmplitude[1]*forceVectorScaling line[5]=contactPt[2]+forceDirectionAndAmplitude[2]*forceVectorScaling line[6]=contactPt[3]+forceDirectionAndAmplitude[3]*forceVectorScaling simAddDrawingObjectItem(lineContainer,line) line[4]=contactPt[1]-forceDirectionAndAmplitude[1]*forceVectorScaling line[5]=contactPt[2]-forceDirectionAndAmplitude[2]*forceVectorScaling line[6]=contactPt[3]-forceDirectionAndAmplitude[3]*forceVectorScaling simAddDrawingObjectItem(lineContainer,line) simAddDrawingObjectItem(sphereContainer,line) index=index+1 else break end end end
再看看接触力动态变化的例子。在场景中添加一个球体，拖离地面，然后双击其图标打开Dynamic属性对话框。点击"Edit material"按钮，在对应的物理引擎下修改材料属性。仿真时使用的是Bullet V2.78，这里修改了恢复参数（值越大材料弹性越大）和摩擦参数。注意，当计算两个物体之间的摩擦时，必须联合二者的摩擦参数（Two colliding objects will have a combined friction value of value1*value2. This does not correspond to the real friction coefficient）。恢复值通常设置在0到1之间，0意味着小球落地不会弹起，即非弹性碰撞（塑性碰撞，碰撞结束时物体变形无任何恢复，动能全部消耗在碰撞过程中），1为完全弹性碰撞。恢复系数一般由实验方法确定，工程中材料的恢复系数可以在工程手册中查到。（Higher restitution values tend to make collisions appear elastic. This does not correspond to the real restitution coefficient. ）
拖入contact display模型显示接触力，开始仿真。小球从一定高度自由落下，碰到地面后反弹几次，最后静止，下面的动图可以看出接触力的变化：
下面一个例子中机械臂末端执行器上装有力传感器，用于测量机械手托举重物的重量。两个关节采用PID控制使机械臂保持在水平位置，单击自定义界面上的Add weight按钮会添加新的重物，对应传感器测得的重量和关节保持力矩都会增大。另外也直接使用了simGetContactInfo函数获取接触力信息并在状态栏输出，从下图可以看到随着重物的增加接触力逐渐变大：
Custom UI:
function buttonClick(ui, id) NewObjectHandle = simCreatePureShape(0, 10 , {0.1,0.1,0.1}, 1) simSetObjectPosition(NewObjectHandle, LastHandle, {0,0,0.2}) LastHandle = NewObjectHandle end if (sim_call_type==sim_childscriptcall_initialization) then InitialObjectHandle = simGetObjectHandle('Cuboid') LastHandle = InitialObjectHandle xml = [[ <ui> <button text="Add weight" onclick='buttonClick'/> </ui> ]] ui=simExtCustomUI_create(xml) end if (sim_call_type==sim_childscriptcall_actuation) then end if (sim_call_type==sim_childscriptcall_sensing) then end if (sim_call_type==sim_childscriptcall_cleanup) then simExtCustomUI_destroy(ui) end
ContactInfoDIsplay:
if (sim_call_type==sim_childscriptcall_initialization) then black={0,0,0} purple={1,0,1} lightBlue={0,1,1} options=0 forceVectorScaling=simGetScriptSimulationParameter(sim_handle_self,'forceVectorScaling') forceVectorWidth=simGetScriptSimulationParameter(sim_handle_self,'forceVectorWidth') contactPointSize=simGetScriptSimulationParameter(sim_handle_self,'contactPointSize') overlayDisplay=simGetScriptSimulationParameter(sim_handle_self,'overlayDisplay') if (overlayDisplay) then options=options+sim_drawing_overlay end -- Add a line and a sphere container: lineContainer=simAddDrawingObject(sim_drawing_lines+options,forceVectorWidth,0,-1,1000,black,black,black,purple) sphereContainer=simAddDrawingObject(sim_drawing_spherepoints+options,contactPointSize,0,-1,1000,black,black,black,lightBlue) ObjectHandle = simGetObjectHandle("Pad") end if (sim_call_type==sim_childscriptcall_cleanup) then -- Remove the containers: simRemoveDrawingObject(lineContainer) simRemoveDrawingObject(sphereContainer) end if (sim_call_type==sim_childscriptcall_sensing) then -- empty the containers: simAddDrawingObjectItem(lineContainer,nil) simAddDrawingObjectItem(sphereContainer,nil) -- Fill the containers with contact information: index=0 sum = 0 while (true) do objectsInContact,contactPt,forceDirectionAndAmplitude=simGetContactInfo(sim_handle_all,ObjectHandle,index) if (objectsInContact) then sum = sum + forceDirectionAndAmplitude[3] if (index == 39) then str = string.format("%f N", sum/10) simAddStatusbarMessage(str) -- Adds a message to the status bar end line={contactPt[1],contactPt[2],contactPt[3],0,0,0} line[4]=contactPt[1]-forceDirectionAndAmplitude[1]*forceVectorScaling line[5]=contactPt[2]-forceDirectionAndAmplitude[2]*forceVectorScaling line[6]=contactPt[3]-forceDirectionAndAmplitude[3]*forceVectorScaling simAddDrawingObjectItem(lineContainer,line) simAddDrawingObjectItem(sphereContainer,line) index=index+1 else break end end end
参考：
Video Game Physics Tutorial - Part III: Constrained Rigid Body Simulation
