Generic 6 Dof Constraint motor bug, and possible fix

[LEgregius]

I've been working with the Generic 6 DOF joint, the btGeneric6DofSpringConstraint to be exact, and I've found that the joint has a tendency to get locked at the limit, specifically the limit where the spring should have the most force. That is, I've been using the spring link to make a sort of suspension, so the sprung mass test to want to compress the spring. Once it hits the limit the constraint just sits at, or just outside the limet.

I tracked the problem down to function

get_limit_motor_info2

specifically the section

Code: Select all

        if (powered)
        {
			info->cfm[srow] = limot->m_normalCFM;
            if(!limit)
            {
				btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

				btScalar mot_fact = getMotorFactor(	limot->m_currentPosition, 
													limot->m_loLimit,
													limot->m_hiLimit, 
													tag_vel, 
													info->fps * limot->m_stopERP);
				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
                info->m_upperLimit[srow] = limot->m_maxMotorForce;
            }
        }

In this case, the motor is not applied when the joint is outside the limit. The problem is my case is that at this point, the target velocity used by the spring, which is implemented using the motor, marked by the boolean named "powered", is greater than that of the constraint error correction, however that is not getting applied because of the if(!limit) check.

taking out the if doesn't work because it messes up the lower and upper limits.

Just below this section is another section that begins with:

Code: Select all

 if(limit)
 {

I moved the

Code: Select all

if(powered)

section below that and changed it to

Code: Select all

        if (powered)
        {
            info->cfm[srow] = 0.0f;
            btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

            btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
                                       limot->m_loLimit,
                                       limot->m_hiLimit,
                                       tag_vel,
                                       info->fps * info->erp);
            btScalar error = mot_fact * limot->m_targetVelocity;
            if (btFabs(error) > btFabs(info->m_constraintError[srow]))
            {
                info->m_constraintError[srow] = error;
            }
            if(!limit)
            {
               info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
               info->m_upperLimit[srow] = limot->m_maxMotorForce;
            }
            else
            {
                if (-limot->m_maxMotorForce < info->m_lowerLimit[srow])
                   info->m_lowerLimit[srow] = -limot->m_maxMotorForce;

                if (limot->m_maxMotorForce > info->m_upperLimit[srow])
                   info->m_upperLimit[srow] = limot->m_maxMotorForce;
            }
        }

In this case, I apply the constraint error that is greater, and if not using a limit, I set the upper and lower limit to the max motor force. If it using the limit, I set the upper and lower limits to be the more extreme between what is already set, and the max motor force to account for the fact that the limit can set the lower and upper limits to -SIMD_INFINITY and SIMD_INFINITY respectively.

I'm not sure if this is the right solution, but it certainly works for me.

If this seems correct, I'll post it as a patch. If not, I can change it to work as need be.

[rponomarev]

Hello,


Thank you for pointing to the problem, looks like springs with limits could have the lock problem.
However your code may introduce new artifacts:

Actually getMotorFactor() should return 0 if constraint is outside limit
so the condition

Code: Select all

 btScalar error = mot_fact * limot->m_targetVelocity;
 if (btFabs(error) > btFabs(info->m_constraintError[srow]))
 {
       info->m_constraintError[srow] = error;
 }

should be always true

Code

Code: Select all

                if (-limot->m_maxMotorForce < info->m_lowerLimit[srow])
                   info->m_lowerLimit[srow] = -limot->m_maxMotorForce;

                if (limot->m_maxMotorForce > info->m_upperLimit[srow])
                   info->m_upperLimit[srow] = limot->m_maxMotorForce

will constantly apply force against the limit.
As a result, the actual position of the constraint will be greater than the upper limit
(or lower than lower limit). If that works for your task, it is OK, but in general getMotorFactor() was especially written to prevent that.

Probably we need a special parameter to control interaction of motor and limit, I'll think about it

Thanks again,
Roman

[LEgregius]

In my case, the the motor factor isn't becoming 0 because the motor should be working with the joint constraint. Clearly the motor should not be able to break the constraint.

I changed the code to this.

Code: Select all

        if (powered)
        {
            info->cfm[srow] = 0.0f;
            btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

            btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
                                       limot->m_loLimit,
                                       limot->m_hiLimit,
                                       tag_vel,
                                       info->fps * info->erp);
            btScalar error = mot_fact * limot->m_targetVelocity;

            if (limit)
            {
                if (info->m_constraintError[srow] < 0.0 && error < info->m_constraintError[srow])
                {
                    info->m_constraintError[srow] = error;
                }
                else if (info->m_constraintError[srow] > 0.0 && error > info->m_constraintError[srow])
                {
                    info->m_constraintError[srow] = error;
                }
            }
            else
            {
               info->m_constraintError[srow] = error;
               info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
               info->m_upperLimit[srow] = limot->m_maxMotorForce;
            }
        }

In this version, the upper and lower limit only get set if they limit is not broken. I changed the code above that to only apply the motor "error" if the constraint error is the same sign and the motor is of greater magnitude. This means that the motor will never cause it to break the constraint.

In my case, the motor, even when the constrant was slightly broken, had a much larger error, several times in fact, over the one the correct for the limit.

I think this should fix any concerns you may have had.

[rponomarev]

Hello,

I found that your code could cause instability in some cases.
Suppose we have a small constraint error (i.e. constraint is slightly broken) and motor acts to move constraint frame away from that limit.
In this case your code will apply the full motor impulse (which is much larger than impulse from constraint error, as you noticed).
But limit in this case will be set to SIMD_INFINITY. As a result the body will get a huge impulse.
Actually in my experiment body was pushed up to the opposite limit during one simulation step (1/60 sec).

You could prevent constraint locking at the limit by setting the stopCFM to some positive value greater than zero.
This makes hard limit more "springy" and adds stability.
Value of 1.0E-5 worked fine for me:

Code: Select all

		btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
		pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.));
		pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.));
		pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, 0.f));
		pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 0.f));
		m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
		pGen6DOFSpring->enableSpring(0, true);
		pGen6DOFSpring->setStiffness(0, 39.478f); // 1 second period for 1kg body
		pGen6DOFSpring->setDamping(0, 0.5f);
		pGen6DOFSpring->getTranslationalLimitMotor()->m_enableMotor[0] = true;
		pGen6DOFSpring->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
		pGen6DOFSpring->getTranslationalLimitMotor()->m_maxMotorForce[0] = 1.f;
		pGen6DOFSpring->setParam(BT_CONSTRAINT_STOP_CFM, 1.0e-5f, 0);
		pGen6DOFSpring->setEquilibriumPoint();

Thanks,
Roman

[LEgregius]

Okay, I updated to bullet 2.76 so I could set the params.

setting STOP_CFM and even fiddling with STOP_ERP as well doesn't seem to help for me. I'm using a rotational axis (axis 5)

My values, using your format.

btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
pGen6DOFSpring->setLinearUpperLimit(btVector3(0., 0., 0.));
pGen6DOFSpring->setLinearLowerLimit(btVector3(0., 0., 0.));
pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -0.05f));
pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 0.05f));
m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
pGen6DOFSpring->enableSpring(5, true);
pGen6DOFSpring->setStiffness(5, 80018.12f); // 1 second period for 1kg body
pGen6DOFSpring->setDamping(0, 1.0f); // This damping doesn't work for me, so I'm simulating a standard viscous damper F = Kd*Vel
//doesn't the spring setup the motor?
//pGen6DOFSpring->getTranslationalLimitMotor()->m_enableMotor[0] = true;
//pGen6DOFSpring->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
//pGen6DOFSpring->getTranslationalLimitMotor()->m_maxMotorForce[0] = 1.f;
pGen6DOFSpring->setParam(BT_CONSTRAINT_STOP_CFM, 1.0e-5f, 0);
//pGen6DOFSpring->setEquilibriumPoint();
// I subclassed it so I could set the equilibrium point directly
pGen6DOFSpring->setEquilibriumPoint(5, 0.10282796);

In may case, the equilibrium point is outside the movement range, so there is still some force when the position is at 0.05, but that's not where I get the problem. There is a sprung mass sitting on 4 similar spring (a car) and the mass in this particular case is 1859.0, so dividing by 4 should be fine. If you add enough force to compress the spring all the way so the joint goes all the way to -0.05, it gets stuck there with a value of something like -0.0500016.

Setting CFM to 1.0e-3f didn't work any better. CFM at 0.1 made the joint just get stuck even more outside the range. Setting it to 1.0e-6f didn't fix it either.

On an unrelated note, the damping value of 0.0-1.0 doesn't seem to correspond to a normal viscous damper, and no value I could set seemed to make the springs settle down. It seems like this number is relative to the step rate, but nothing I tried really helped. It looked like it wouldn't be hard to change this to working as F=Kd*V. Do you have any thoughts or opinions on this?

[LEgregius]

Doing the same thing with the powered block against the 2.76 code base, I changed it so that it sets the lower and upper limits if the motor force is greater and the value was infinity. Hopefully, this should clear up the anomaly you were seeing.

It also seems like it may be a bit of hack what I did, but it still seems like the motor "correction" should win if it's greater than the limit correction.

I tried fiddling with both bounce and cfm, and found that I could make it stop getting stuck only if I made the bounce really huge, something like 10-15, but it still ended up with a sort of sticky behavior, it would just break out of it more easily, but it was still unacceptable, and very unstable.

I tried again with this version, where I don't allow the motor force to be applied at the same time as an infinity value. Here it resets the infinity values to the max motor velocity if the motor "error" is greater. This still works very well for me, and should fix your issue.

Code: Select all

        if (powered)
        {
        	info->cfm[srow] = limot->m_normalCFM;
        	btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

        	btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
        			limot->m_loLimit,
        			limot->m_hiLimit,
        			tag_vel,
        			info->fps * info->erp);
        	btScalar error = mot_fact * limot->m_targetVelocity;

        	if (limit)
        	{
        		if ((info->m_constraintError[srow] < 0.0 && error < info->m_constraintError[srow]) ||
        				(info->m_constraintError[srow] > 0.0 && error > info->m_constraintError[srow]))
        		{
        			info->m_constraintError[srow] = error;
        			if (info->m_lowerLimit[srow] == -SIMD_INFINITY)
        				info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
        			if (info->m_upperLimit[srow] == SIMD_INFINITY)
        				info->m_upperLimit[srow] = limot->m_maxMotorForce;
        		}
        	}
        	else
        	{
        		info->m_constraintError[srow] = error;
        		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
        		info->m_upperLimit[srow] = limot->m_maxMotorForce;
        	}
        }

[rponomarev]

Hello,

I tried your code, it seems that it will break constraint limits if stiffness of spring is large enough,
so error from motor is always greater than error from constraint position limit,
which is info->fps * limot->m_stopERP * limot->m_currentLimitError
In that case the example from my previous post works as there are no limits on X axis at all

About dumping and CFM : looks like there are errors in your code
It should be

Code: Select all

pGen6DOFSpring->setDamping(5, 0.01f);  // use axis 5 and small value to make it viscous
pGen6DOFSpring->setParam(BT_CONSTRAINT_STOP_CFM, 1.0e-5f, 5);  // use axis 5 

I also tried 2.76 ConstraintDemo code with your parameters, looks like it works.
I had to set

Code: Select all

btScalar mousePickClamping = 1111111130.f;

in DemoApplication.cpp to be able to manipulate such a strong spring

Code: Select all

		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
		btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
		pBodyA->setActivationState(DISABLE_DEACTIVATION);

		tr.setIdentity();
		tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
		tr.getBasis().setEulerZYX(0,0,0);
		btRigidBody* pBodyB = localCreateRigidBody(1859.0, tr, shape);
		pBodyB->setActivationState(DISABLE_DEACTIVATION);

		btTransform frameInA, frameInB;
		frameInA = btTransform::getIdentity();
		frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.)));
		frameInB = btTransform::getIdentity();
		frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));

		btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
		pGen6DOFSpring->setLinearUpperLimit(btVector3(0., 0., 0.));
		pGen6DOFSpring->setLinearLowerLimit(btVector3(0., 0., 0.));
		pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -0.05f));
		pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 0.05f));
		m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
		pGen6DOFSpring->enableSpring(5, true);
		pGen6DOFSpring->setStiffness(5, 80018.12f); 
		pGen6DOFSpring->setDamping(5, 0.01f);
		pGen6DOFSpring->setParam(BT_CONSTRAINT_STOP_CFM, 1.0e-5f, 5);
		// I subclassed it as well for test
		pGen6DOFSpring->setEquilibriumPoint(5, 0.10282796);

Thanks,
Roman

[LEgregius]

Oh, the CFM bug was a mistake from where I copied my values into you code so you could test it. My version did set axis 5. I also verified in the debugger that the right CFM value was set. Same thing when I tried also setting bounce.

I have the damping turned off because I'm externally generating a damper using a btAction so I can calculate it as F=Kd * AngularVel.

I don't know how to calculate what the 0-1 setting should be if I start with a fraction of 0-1 of critical damping. If you can tell me how to calculate that, I'll use it that way.

I'll look at it in DemoApplication and see what happens.

[LEgregius]

I played with it in the constraint demo, and I actually couldn't get it to lock at all there, but I believe that the reason for this comes down to the simple fact that my version has 0.25 * 1859 * gravity newtons compressing it all the time. With that much force pressing down, the extra springiness that CFM gives it really doesn't make any difference.

I do see the problem in the demo app with it breaking the constraint, though, and I do see how the reverse of giving the spring correction infinite max on the limit is a problem. I think I understand better what that code is doing. It should behave in exactly the same way that it would if you had two constraints, one with just the spring, and on with just the limit. It needs a way to set both the correction error with a limit of infinity and a spring with a limit based on the max velocity. I can just add two constraints, but it seems that I shouldn't have to.

I don't know if there is a mathematical way to combine the two so that it will yield the same result as having two, and I don't have the engineering hours at the moment to try to solve that.