Why is the ScrewJoint not stable?

[pigbobdog]

Hi,

I've implemented a

ScrewJoint

to simulate the movement of a screw. There is an attribute in the class called - ratio, The attribute 'ratio' means that how much distance screw joint moves in one rotational circle. The unit for ratio is millimeter.

The definition of the ScrewJoint in C++ is:

Code: Select all

class btScrewConstraint : public btSliderConstraint {
        public:
            btScrewConstraint (btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA, const btScalar& ratio);
            btScrewConstraint (btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, 
                                      const btTransform& frameInB, bool useLinearReferenceFrameA, const btScalar& ratio);
            ~btScrewConstraint ();

            void setRatio (const btScalar& ratio)
            { m_fRatio = ratio; }
            const btScalar& getRatio ()
            { return m_fRatio;}
            void setAxis(btAxis *pAng, btAxis *pLin)
            {
                m_pAngularAxis = pAng;
                m_pLinearAxis = pLin;
            }
            void setFunc (btAxisFunc* pFunc) { m_pFunc = pFunc; }

            // internal method used by the constraint solver, don't use them directly
            virtual void getInfo1 (btConstraintInfo1* info);
            virtual void getInfo2 (btConstraintInfo2* info);

        protected:
            btAxis *m_pAngularAxis;
            btAxis *m_pLinearAxis;
            btAxisFunc *m_pFunc;
            btScalar m_fRatio;
        };

The implementation for ScrewJoint is as follows:

Code: Select all

btScrewConstraint::btScrewConstraint (btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, 
                                      const btTransform& frameInB, bool useLinearReferenceFrameA, const btScalar& ratio) :
btSliderConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA),
m_fRatio(ratio)
{
    m_pAngularAxis = NULL;
    m_pLinearAxis = NULL;
    m_pFunc = NULL;
}

btScrewConstraint::btScrewConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA, const btScalar& ratio) :
btSliderConstraint(rbB, frameInB, useLinearReferenceFrameA),
m_fRatio(ratio)
{
    m_pAngularAxis = NULL;
    m_pLinearAxis = NULL;
    m_pFunc = NULL;
}

btScrewConstraint::~btScrewConstraint ()
{
    m_pAngularAxis = NULL;
    m_pLinearAxis = NULL;
    m_pFunc = NULL;
}

void btScrewConstraint::getInfo1(btConstraintInfo1* info)
{
    btSliderConstraint::getInfo1(info);
    double fMult = m_fRatio;
    if (!EQ_is_zero(fMult, EQ_ask_systol))
    {
        info->m_numConstraintRows = 5;
        info->nub = 1;
    }
}
void btScrewConstraint::getInfo2(btConstraintInfo2* info)
{
    btSliderConstraint::getInfo2(info);

    btVector3 vecAxis = m_pLinearAxis->getAxis();
    btVector3 vecAxis2 = m_pAngularAxis->getAxis ();
    btVector3 vecAngPos = m_pAngularAxis->getPosition();
    if (!EQ_is_zero(m_fRatio, EQ_ask_systol))
    {
        btVector3 vec3Result;
        btScalar coefficient;
        m_pFunc->calc(0.0, vec3Result, coefficient);
        btVector3 vec1 = vecAxis * -coefficient;
        btVector3 vec2 = vecAxis2;

        int s4 = 4 * info->rowskip;
        info->m_J1linearAxis[s4 + 0] = vec1[0];
        info->m_J1linearAxis[s4 + 1] = vec1[1];
        info->m_J1linearAxis[s4 + 2] = vec1[2];
        info->m_J1angularAxis[s4 + 0] = vec2[0];
        info->m_J1angularAxis[s4 + 1] = vec2[1];
        info->m_J1angularAxis[s4 + 2] = vec2[2];

        info->m_J2angularAxis[s4 + 0] = vec2[0];
        info->m_J2angularAxis[s4 + 1] = vec2[1];
        info->m_J2angularAxis[s4 + 2] = vec2[2];
        info->m_J2linearAxis[s4 + 0] = vec1[0];
        info->m_J2linearAxis[s4 + 1] = vec1[1];
        info->m_J2linearAxis[s4 + 2] = vec1[2];

        btScalar fY = vec3Result.m_floats[1] * vecAngPos.m_floats[0] - vec3Result.m_floats[0] * vecAngPos.m_floats[1];
        btScalar fX = vec3Result.m_floats[0] * vecAngPos.m_floats[0] + vec3Result.m_floats[1] * vecAngPos.m_floats[1];
        btScalar fDiff = btAtan2 (fY, fX);
        info->m_constraintError[s4] = fDiff * info->fps * info->erp;
    }
}

And the calculation method calc is implemented in :

Code: Select all

void ScrewFuncWrapBT::calc (btScalar dt, btVector3& vec3Result, btScalar& coeff)
{
    btVector3 vecLinPos = m_pLinearAxis->getPosition ();
    double linPos = vecLinPos.m_floats[0];
    double times = linPos / m_fRatio;
    double curPos = times * PI * 2;
    BulletMath::CopyToBullet(MDPHYS_CalcVector(curPos), vec3Result);
    coeff = (2 * PI) / m_fRatio / m_fScale;
}

In runtime I observe that when applying the angular speed to the ScrewJoint, it is always stable, but if applying linear speed and the ratio is small enough, for example 5mm, the behavior is wrong. I don't know what's the reason.

It would be much appreciated if you could help me to solve the issue.