在VREP中的物體上添加一個力矩看看它會怎么運動。新建一個場景，向其中添加一個立方體，將立方體抬高離開地面，並在Dynamics選項卡中將重力設置為0. 然后在立方體上施加一個力矩T={1,1,1}，立方體將在力矩作用下旋轉起來：
function sysCall_init() -- do some initialization here: handle=sim.getObjectHandle('Cuboid') graphHandle = sim.getObjectHandle('Graph') pos = sim.getObjectPosition(handle, -1) torque = {1, 1, 1} lineContainer = sim.addDrawingObject(sim.drawing_lines, 3, 0, -1, 99999, {0.2,0.2,0.2}) lineDat = {pos[1],pos[2], pos[3], pos[1]+0.1*torque[1], pos[2]+0.1*torque[2], pos[3]+0.1*torque[3]} sim.addDrawingObjectItem(lineContainer, lineDat) end function sysCall_actuation() -- put your actuation code here sim.addForceAndTorque(handle,nil,torque) end function sysCall_sensing() -- put your sensing code here _, angularVel = sim.getVelocity(handle) --print(angularVel[1],angularVel[2],angularVel[3]) sim.setGraphUserData(graphHandle,'vx',angularVel[1]) sim.setGraphUserData(graphHandle,'vy',angularVel[2]) sim.setGraphUserData(graphHandle,'vz',angularVel[3]) end
VREP中給物體施加力有兩個函數sim.addForce和sim.addForceAndTorque. 其中sim.addForceAndTorque施加的有心力（central force：有心力的方向永遠指向一個固定點或動點）作用在物體質心上，並且以世界坐標系為參照進行描述（they are expressed as absolute values, i.e. relative to the world reference frame）。而sim.addForce施加非有心力（non-central force）. Note that the force is applied just for one simulation pass. If you want a specific force to be constantly present, you will have to add it again in each simulation step. Remember that simAddForceAndTorque needs to be called constantly, since the forces added like that are reset at each simulation step.
為了計算和觀察方便，設立方體質量及慣性參數都為1，那么根據歐拉方程，可以算出物體沿三個坐標分量（以世界坐標系為參考）的角加速度值都應該為1rad/s2
VREP中獲取物體的速度和角速度也有兩個函數，分別是sim.getVelocity (return the velocity computed by the physics engine. works only with dynamically simulated shape) 以及sim.getObjectVelocity(returns a velocity computed by V-REP. works with all scene objects)
代碼中使用sim.getVelocity函數獲取物體角速度分量后，通過Graph自定義數據（User data）的方式在圖表上繪制出來。可以看出三個方向的角速度曲線重合，並且斜率均為1
物理引擎中已知作用在物體上的力和力矩，計算物體的運動需要用到牛頓運動定律。對於平動，定義逆質量$m^{-1}$（inverse mass ）將牛頓第二運動定律改寫為：$$\ddot{a}=m^{-1}f$$
對於轉動，定義逆慣性張量$I^{-1}$（inverse inertia tensor ），根據動量矩定力：$$\ddot{\mathbf{\theta}}=I^{-1}\tau $$
求出加速度和角加速度后對其進行積分可得到速度、角速度，進一步積分可得位置和姿態。在Bullet2.78中該過程的實現如下。注意程序中會對角速度進行限制，因為如果角速度過大在碰撞檢測時可能檢測不到碰撞，導致物體間發生穿透。
void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; m_linearVelocity += m_totalForce * (m_inverseMass * step); m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; #define MAX_ANGVEL SIMD_HALF_PI /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); if (angvel*step > MAX_ANGVEL) { m_angularVelocity *= (MAX_ANGVEL/step) /angvel; } }
下圖可以明顯看出角速度增大到一定幅度后被限制在一個最大值上。
對於轉動，這里有一個問題：上面的方程力矩、角加速度 、以及慣性張量都是定義在世界坐標系中的。如果動坐標系（Local axes）的姿態與世界坐標系不一致，那么世界坐標系中描述的慣性張量將隨着物體的姿態不同實時變化。為了能應用上面的方程，需要將動坐標系中的慣性張量轉換到世界坐標系下來表達。
下面進行簡單的推導（參考3D Rigid Body Dynamics: The Inertia Tensor），假設世界坐標系下剛體的動量矩矢量為${L}'$，動坐標系下的動量矩矢量為$L$，動坐標系到世界坐標系的轉換矩陣為$R$。那么:$${L}'=RL=R(I\omega)=RIR^{T}R\omega=(RIR^{T})(R\omega)={I}'{\omega}'$$
因為旋轉矩陣R為正交對稱矩陣，所以有$RR^{T}=Identity$，$Identity$是單位矩陣。因此，將慣性張量從局部坐標系轉換到世界坐標系下的變換為：$${I}'=RIR^{T}$$
對於旋轉矩陣$R^{T}=R^{-1}$則，$${I}'^{-1}=(RIR^{T})^{-1}=(R^{T})^{-1}I^{-1}R^{-1}=RI^{-1}R^{T}$$
Bullet物理引擎中計算世界坐標系下物體逆慣性張量矩陣的函數如下。可以看出局部坐標系下的逆慣性張量矩陣在設置物體質量屬性的時候就已經計算好了，局部坐標系一般為物體的慣性主軸坐標系，該坐標系下慣性張量矩陣只有主對角線上有元素，計算其逆矩陣只需對主對角線元素求倒數。
void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { //... //... m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); //... } void btRigidBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); }
另外出現了一個新的問題，我們將角加速度積分得到世界坐標系下描述的角速度后怎么樣將其表示成物體姿態？（We need to update the orientation by the angular velocity）因為后面計算旋轉矩陣R需要知道物體當前姿態，否則迭代就沒法進行。
在計算機圖形學和剛體旋轉的研究中經常用四元數來表示物體的姿態。下面的公式將絕對坐標系下的角速度與四元數的微分聯系起來（證明可以參考Quaternion Calculus以及Quaternion and Rotation）：
$$\dot{q}(t)=\frac{1}{2}W(t)*q(t)=\frac{1}{2}(\omega_x(t)i+\omega_y(t)j+\omega_z(t)k)*q(t)$$
其中，$q(t)$為代表物體姿態隨時間變化的四元數，W為角速度構造出的實部為零的四元數：$W(t)=(0,\omega_x(t),\omega_y(t),\omega_z(t))$，“*”表示四元數乘法。
這一公式簡單的證明過程如下：
在$t+\Delta t$時刻，物體姿態可由四元數$q(t+\Delta t)$表示。在$\Delta t$時間段內物體繞着瞬時軸$\hat{\omega}=\omega/||\omega||$從$q(t)$運動到$q(t+\Delta t)$，這一時間段內的角位移為$\Delta \theta=||\omega||\Delta t$。根據四元數與Axis-Angle之間的關系可知：
$$\begin{align*}
\Delta q &=cos\frac{\Delta \theta}{2}+\hat{\omega}sin\frac{\Delta \theta}{2}\\
&=cos\frac{||\omega||\Delta t}{2}+\hat{\omega}sin\frac{||\omega||\Delta t}{2}
\end{align*}$$
在$\Delta t$時間段內姿態的變換也可以通過四元數乘法（與旋轉矩陣乘法意義類似）得到：$$q(t+\Delta t)=\Delta q* q(t)$$
現在我們開始推導$\dot{q}(t)$：
$$\begin{align*}
q(t+\Delta t)-q(t)&=(cos\frac{||\omega||\Delta t}{2}+\hat{\omega}sin\frac{||\omega||\Delta t}{2})q(t)-q(t)\\
&=-2sin^2 \frac{||\omega||\Delta t}{4}q(t)+\hat{\omega}sin\frac{||\omega||\Delta t}{2}q(t)
\end{align*}$$
上面式子中第一項是關於$\Delta t$的高階無窮小，因此Δt→0時為零。所以有：
$$\begin{align*}
\dot{q}(t)&=\lim_{\Delta t \to 0}\frac{q(t+\Delta t)-q(t)}{\Delta t}\\
&=\hat{\omega}\lim_{\Delta t \to 0}\frac{sin(||\omega||\Delta t/2)}{\Delta t}q(t)\\
&=\hat{\omega}\frac{d}{dt}sin(\frac{||\omega||t}{2})|_{t=0}q(t)\\
&=\hat{\omega}\frac{||\omega||}{2}q(t)\\
&=\frac{1}{2}\omega(t)q(t)
\end{align*}$$
上面公式寫成離散形式如下：$$q_{t+\Delta t}=q_t+\frac{\Delta t}{2}W_t*q_t$$
根據這一公式可以用角速度更新物體的姿態，具體的實現可以參考cyclone-physics物理引擎（與Game Physics Engine Development一書相配套）源代碼中core.h頭文件的第592行：
class Quaternion { public: union { struct { /** * Holds the real component of the quaternion. */ real r; /** * Holds the first complex component of the * quaternion. */ real i; /** * Holds the second complex component of the * quaternion. */ real j; /** * Holds the third complex component of the * quaternion. */ real k; }; /** * Holds the quaternion data in array form. */ real data[4]; }; // ... other Quaternion code as before ... /** * The default constructor creates a quaternion representing * a zero rotation. */ Quaternion() : r(1), i(0), j(0), k(0) {} /** * The explicit constructor creates a quaternion with the given * components. * * @param r The real component of the rigid body's orientation * quaternion. * * @param i The first complex component of the rigid body's * orientation quaternion. * * @param j The second complex component of the rigid body's * orientation quaternion. * * @param k The third complex component of the rigid body's * orientation quaternion. * * @note The given orientation does not need to be normalised, * and can be zero. This function will not alter the given * values, or normalise the quaternion. To normalise the * quaternion (and make a zero quaternion a legal rotation), * use the normalise function. * * @see normalise */ Quaternion(const real r, const real i, const real j, const real k) : r(r), i(i), j(j), k(k) { } /** * Normalises the quaternion to unit length, making it a valid * orientation quaternion. */ void normalise() { real d = r*r+i*i+j*j+k*k; // Check for zero length quaternion, and use the no-rotation // quaternion in that case. if (d < real_epsilon) { r = 1; return; } d = ((real)1.0)/real_sqrt(d); r *= d; i *= d; j *= d; k *= d; } /** * Multiplies the quaternion by the given quaternion. * * @param multiplier The quaternion by which to multiply. */ void operator *=(const Quaternion &multiplier) { Quaternion q = *this; r = q.r*multiplier.r - q.i*multiplier.i - q.j*multiplier.j - q.k*multiplier.k; i = q.r*multiplier.i + q.i*multiplier.r + q.j*multiplier.k - q.k*multiplier.j; j = q.r*multiplier.j + q.j*multiplier.r + q.k*multiplier.i - q.i*multiplier.k; k = q.r*multiplier.k + q.k*multiplier.r + q.i*multiplier.j - q.j*multiplier.i; } /** * Adds the given vector to this, scaled by the given amount. * This is used to update the orientation quaternion by a rotation * and time. * * @param vector The vector to add. * * @param scale The amount of the vector to add. */ void addScaledVector(const Vector3& vector, real scale) { Quaternion q(0, vector.x * scale, vector.y * scale, vector.z * scale); q *= *this; r += q.r * ((real)0.5); i += q.i * ((real)0.5); j += q.j * ((real)0.5); k += q.k * ((real)0.5); } void rotateByVector(const Vector3& vector) { Quaternion q(0, vector.x, vector.y, vector.z); (*this) *= q; } };
將上面公式結合起來，用Mathematica編程計算物體在力矩下的運動。仿真5秒，時間步長為1毫秒：
繪制出世界坐標系下的角速度曲線如下：
在VREP中選bullet2.78或Newton物理引擎進行仿真，結果與上面的計算一致：
但是當選擇bullet2.83或ODE物理引擎時計算結果與上面的不同：
用ADAMS、SIMPACK等更精確的動力學仿真軟件進行模擬，得到的結果也與bullet2.83一致。對下面同樣的代碼，bullet2.78(2.82)和bullet2.83給出的結果完全不同，實際上bullet2.83的結果是更准確的。在某些游戲物理引擎中，為了保證計算的實時性或穩定性對物理過程進行了一定的簡化，省去了實際動量矩定理公式中的某些復雜的項。
#include <btBulletDynamicsCommon.h> #include <stdio.h> #include <iostream> #include <fstream> using namespace std; /// This is a Hello World program for running a basic Bullet physics simulation int main(int argc, char** argv) { btBroadphaseInterface* broadphase = new btDbvtBroadphase(); ///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration. btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a differnt dispatcher btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); ///the default constraint solver. For parallel processing you can use a different solver btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; ///instantiate the dynamics world btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration); ///sets the gravity dynamicsWorld->setGravity(btVector3(0, 0, 0)); btCollisionShape* Shape = new btSphereShape(1); //The btTransform class supports rigid transforms with only translation and rotation btDefaultMotionState* MotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 50, 0))); btScalar mass = 1; btVector3 Inertia(2, 1, 1); btVector3 torque(1, 1, 1); btVector3 angularVelocity(0, 0, 0); ///when bodies are constructed, they are passed certain parameters. This is done through a special structure Bullet provides for this. ///rigid body is dynamic if and only if mass is non zero, otherwise static btRigidBody::btRigidBodyConstructionInfo RigidBodyCI(mass, MotionState, Shape, Inertia); btRigidBody* RigidBody = new btRigidBody(RigidBodyCI); dynamicsWorld->addRigidBody(RigidBody); ofstream outfile("data.csv", ios::out); for (int i = 0; i < 300; i++) { RigidBody->applyTorque(torque); dynamicsWorld->stepSimulation(1 / 60.f, 10); angularVelocity = RigidBody->getAngularVelocity(); outfile << angularVelocity.getX() << "," << angularVelocity.getY() << "," << angularVelocity.getZ() << endl; } outfile.close(); delete Shape; delete dynamicsWorld; delete solver; delete dispatcher; delete collisionConfiguration; delete broadphase; printf("Press a key to exit\n"); getchar(); }
接下來我們將進行正確的公式推導和轉動模擬。
參考：
Modern Robotics Mechanics, Planning, and Control Chapter 8. Dynamics of Open Chains
Game Physics Engine Development（https://github.com/idmillington/cyclone-physics）
Overview of core principles behind real-time physics systems