物理引擎中的剛體轉動1

時間 2019-11-06

標籤物理引擎剛體轉動简体版

原文原文鏈接

　　在VREP中的物體上添加一個力矩看看它會怎麼運動。新建一個場景，向其中添加一個立方體，將立方體擡高離開地面，並在Dynamics選項卡中將重力設置爲0. 而後在立方體上施加一個力矩T={1,1,1}，立方體將在力矩做用下旋轉起來：html

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

View Code

　　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. ios

　　爲了計算和觀察方便，設立方體質量及慣性參數都爲1，那麼根據歐拉方程，能夠算出物體沿三個座標份量（以世界座標系爲參考）的角加速度值都應該爲1rad/s²git

　　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)github

　　代碼中使用sim.getVelocity函數獲取物體角速度份量後，經過Graph自定義數據（User data）的方式在圖表上繪製出來。能夠看出三個方向的角速度曲線重合，而且斜率均爲1web

　　物理引擎中已知做用在物體上的力和力矩，計算物體的運動須要用到牛頓運動定律。對於平動，定義逆質量$m^{-1}$（inverse mass ）將牛頓第二運動定律改寫爲：$$\ddot{a}=m^{-1}f$$express

　　對於轉動，定義逆慣性張量$I^{-1}$（inverse inertia tensor ），根據動量矩定力：$$\ddot{\mathbf{\theta}}=I^{-1}\tau $$編程

　　求出加速度和角加速度後對其進行積分可獲得速度、角速度，進一步積分可得位置和姿態。在Bullet2.78中該過程的實現以下。注意程序中會對角速度進行限制，由於若是角速度過大在碰撞檢測時可能檢測不到碰撞，致使物體間發生穿透。app

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;
    }

}

View Code

　　下圖能夠明顯看出角速度增大到必定幅度後被限制在一個最大值上。ide

　　對於轉動，這裏有一個問題：上面的方程力矩、角加速度、以及慣性張量都是定義在世界座標系中的。若是動座標系（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;
        }
    };

View Code

　　將上面公式結合起來，用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();
}