Soft constraint derivation

Please don't post Bullet support questions here, use the above forums instead.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Soft constraint derivation

Post by c0der »

So far, I have the following equations for the soft constraint general form

JM^-1JT*lambda*dt = - Jv - beta/dt*C - gamma*lambda

Lambda is currently a force

To turn lambda into an impulse, i multiply by dt

(JM^-1JT*dt + gamma)*lambda' = -Jv*dt - beta*C

beta = ERP = (k*dt)/(c+k*dt)
gamma = CFM = 1/(c + k*dt)

k is 2*PI*wn (rad/s) and c is dimensionless (<1 under-damped 1 critically-damped >1 over-damped)

This gives me erratic behaviour when warm-starting, anyone experience this problem?
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Soft constraint derivation

Post by Dirk Gregorius »

Can I see your how you apply your constraints? The devil is in the detail here.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

Thanks for the quick reply Dirk. I will try to make this as clear and detailed as possible for a simple distance joint

For the distance joint constraint, I do this:

Code: Select all

P.zero(); // Zero the accumulated impulse

PreStep() :

C = |xa + ra - xb - rb| - InitialDistance

A = n.dot(InvMa*n + SkewRaT*InvIa*n*SkewRa  + InvMb*n + SkewRbT*InvIb*n*SkewRb);

k = A * wn * wn; // Spring const (wn = 2*PI * wn(Hz))
c = 2 * A * wn * zeta; // Damping ratio

ERP =  (dt*k) / c+dt*k)
CFM = 1/(c+dt*k) otherwise if wn is zero, CFM=1

InvAPlusCFM = 1 / (A + CFM/dt); // Dividing gamma by dt in terms of an impulse

Bias = ERP / dt * C;

if WarmStarting
va += InvMa*n*P
wa += InvIa*n*SkewRa*P
vb -= InvMb*n*P
wb -= InvIb*n*SkewRb*P

ApplyImpluse() :

b = n.negated.dot(va + SkewRaT*wa - vb - SkewRbT*wb) - bias

lambda = b * InvAPlusCFM

P += lambda

va += InvMa*n*lambda
wa += InvIa*n*SkewRa*lambda
vb -= InvMb*n*lambda
wb -= InvIb*n*SkewRb*lambda
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Soft constraint derivation

Post by Dirk Gregorius »

b = -J*v - beta / dt * C - gamma * AccumulatedImpulse;

You cannot pre-compute and have to do this in every iteration. Also note that you can make your effective mass computation a bit simpler. Transpose( Skew ) = -Skew. Also if n is a unit vector nT * invM * n = invM.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

Thanks Dirk. That stabilisation term you added is more clear now. This thread explains in more detail what needs to be done when another constraint messes up the velocity http://bulletphysics.org/Bullet/phpBB3/ ... f=4&t=1354

I am having problems with a straight line joint between two bodies. One body is fixed and the line axis is fixed to the frame of this body. The other body is at the anchor point and moves up and down between two limits. However, sometimes, the body stays at the bottom limit and then starts oscillating again.

Here is the pseudo code for the soft limiting constraint (I haven't simplified the calculations yet), where the line joint itself works fine, and is not a soft constraint:

Prestep (Once per frame)

CLimit = u.dot(relative position) // u is the line axis

if(CLimit>=lowerlimit && CLimit<=UpperLimit)
skip constraint
else
Enforce constraint

ALimit = u*invMa*u + cross(u, rel-ra)*InvIa*cross(u, rel-ra) + u*invMb*u + Cross(rb,u)*invIb* + Cross(rb,u)

k = ALimit * wn * wn
c = 2 * Alimit * wn * zeta

ERP = (dt*k)/(c+dt*k)
CFM = 1/(c+dt*k) if wn!=0

InvALimitPlusCFM = 1 / (ALimit + CFM)

BIasLimit = ERP / dt * CLimit

if(warm starting)
{
va += invMa*u*PLimit
wa += InvIa*Cross(u,rel-ra)*PLimit
vb -= invMb*u*PLimit
wb -= InvIb*Cross(rb,u)*PLimit
}
else {
PLimit = 0;
}

ApplyImpulse() (At each iteration)

bLimit = -u.va - Cross(u,rel-ra)*wa + u.wb + Cross(rb,u)*wb - CFM*PLimit - biasLimit

lambdaLimit = bLimit*InvALimitPlusCFM

PLimit += LambdaLimit

va += invMa*u*LambdaLimit
wa += InvIa*Cross(u,rel-ra)*LambdaLimit
vb -= invMb*u*LambdaLimit
wb -= InvIb*Cross(rb,u)*LambdaLimit
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Soft constraint derivation

Post by Dirk Gregorius »

I use these formulas for CFM and ERP:

CFM = 1.0f / ( dt * ( d + dt * k ) )
ERP = dt * k * CFM = k / ( d + dt * k )

Also (assuming |u| == 1):
u*invMa*u = invMa
u*invMb*u = invMb

You also need to clamp the impulses. If the lower limit is violated you have only positive impulses. If the upper limit is violated you have only negative impulses.

I recommend looking at Box2D into b2MouseJoint.cpp for the formulas of the soft constraint. Erin gave a presentation about this topic at the GDC if you want to read more. For an example of the limits look into b2RevoluteJoint.cpp.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

Thanks Dirk.

I have looked into the derivation as to why you are dividing by h but am definitely missing something, causing my suspension bridge to oscillate with a damping coefficient of 1, so I am going back to deriving the equations.

Keeping warm starting aside and keeping lambda as a force rather than an impulse, I have:

JM^-1JT*h*lambda = -J*vi - beta/h*C - gamma*lambda

Where beta = kh/(d+kh) and gamma=1/(d+kh), they are derived for forces from the harmonic oscillator

So, given lambda is a force now,

(JM^-1JT*h + gamma)*lambda = -J*vi - beta/h*C

So beta/h is in dimensionless units and gamma is in the units s/kg

In units, we have (s/kg + s/kg)*lambda = m/s - m/s

so (m/s)/(s/kg) = kg*m/s^2 where lambda is a force in Newtons

So at each iteration, all I have to do is multiply lambda by dt and it becomes an impulse.

I am definitely doing something wrong, any ideas?
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

(JM^-1JT*h + gamma)*lambda = -J*vi - beta/h*C

Ok, since we have s/kg + s/kg, taking h out of the brackets gives

(JM^-1JT + gamma/h)*lambda*h = -Jvi - beta/h*C

That's good, now lambda can be solved as an impulse

So for my revolute joint, I have

P = 0; lambda = 0;

PreStep() {

C = xa + ra - xb - rb;

A = m_matInvMa + m_matSkewRaT*m_matInvIa*m_matSkewRa + m_matInvMb + m_matSkewRbT*m_matInvIb*m_matSkewRb;

Matrix3x3 SpringConstant = A*m_sWn*m_sWn; // K = A * wn^2
Matrix3x3 DampingCoefficient = A*2*m_sWn*m_sZeta; // C = 2*A*wn*zeta

Matrix3x3 InvDenom;
InvDenom.inverseOf(DampingCoefficient+SpringConstant*dt); // 1/(c+k*dt)

m_matERP = (PhysicsWorld::g_bPositionCorrection) ? (SpringConstant*dt)*InvDenom : I*0.0f;
m_matCFM = (m_sWn==0.0f) ? Identity : InvDenom*(1/dt); // Must be non-zero

Matrix3x3 matInvA, matInvCFM;
matInvA.inverseOf(m_matA);
matInvCFM.inverseOf(m_matCFM);

m_matInvAPlusCFM = matInvA + matInvCFM;

m_vBias = m_matERP * (1/dt) * C;

if(PhysicsWorld::g_bWarmStarting) {
m_pRigidBody1->m_vVelocity += m_matInvMa*m_vP;
m_pRigidBody1->m_vAngularVelocity += m_matInvIa*m_matSkewRa*m_vP;
m_pRigidBody2->m_vVelocity -= m_matInvMb*m_vP;
m_pRigidBody2->m_vAngularVelocity -= m_matInvIb*m_matSkewRb*m_vP;
}
else {
m_vP.zero();
}
}

ApplyImpulse()
{
Vector4 b = va.negated() - m_matSkewRaT*wa + vb + m_matSkewRbT*wb - m_matCFM*m_vP - m_vBias;

m_vLambda = b*m_matInvAPlusCFM;
m_vP += m_vLambda; // Accumulate impulse for warm starting

m_pRigidBody1->m_vVelocity += m_matInvMa*m_vLambda;
m_pRigidBody1->m_vAngularVelocity += m_matInvIa*m_matSkewRa*m_vLambda;
m_pRigidBody2->m_vVelocity -= m_matInvMb*m_vLambda;
m_pRigidBody2->m_vAngularVelocity -= m_matInvIb*m_matSkewRb*m_vLambda;
}

This sends the bodies into an orbit off the screen, even if i disable warm starting
bone
Posts: 231
Joined: Tue Feb 20, 2007 4:56 pm

Re: Soft constraint derivation

Post by bone »

You might do a search on this forum for the same subject on CFM and ERP; I think Dirk and I discussed it previously. IIRC, Box2D uses one derivation (including the h in the coefficients), Bullet does the opposite. Or maybe vice versa. But it can be made to work either way, so if you're deriving it in a way that works, then you're done.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Soft constraint derivation

Post by Dirk Gregorius »

There are two gotchas in the practical implementation of soft constraints. The first is that you use impulses in your solver, but the harmonic oscillator uses forces. This is where the confusion with the timestep h comes from. The formula I posted here is correct if your lambdas are impulses.

The second gotcha is how to solve soft constraints with sequential impulses. You have to write this down very carefully. The sequential impulse update is:

J*M^-1*JT * DeltaLambda = -beta * C / dt - J * v

Here DeltaLambda is an impulse. In the context of soft constraints this is extended to:

J*M^-1*JT * DeltaLambda = -beta * C / dt - J * v - gamma * AccumulatedLambda

Notice the use of the delta and accumulated impulse in the equation. Erin derived this somewhere here in the forum so I suggest searching for ERP/CFM, sequential impulse and soft constraints. Also have a look at Erin's presentation. The formulas in the presentation are for forces though iirc and I understand that this is a reason for confusion. Anyway, the formulas I posted here are correct. If you drop them into your code and they don't work this is a simple bug somewhere in your code. No need to shoot with the big gun on this and re-derive everything :)
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

Thanks Dirk, I know your formulas are definitely correct, but I was trying to understand some concepts. In Box2D lite, Erin doesn't actually divide gamma by the timestep:

float softness = 1.0f / (d + timeStep * k);

He also adds gamma to the diagonal of the mass matrix in the prestep.

My revolute joint was actually working, except I had the spring stiffness set at 2 as in Box2D lite and the bridge fell down the y axis due to gravity. However my span is 4x10m 50kg planks, whereas in Box2D lite it's a 10m span, so I assume I have to increase the stiffness substantially to get a mild parabolic curvature. So I have my natural frequency at 150Hz and damping ratio at 50 to get it to settle fast. If I set the damping ratio to 1, I get oscillation but the bridge eventually settles. Anything under 1.0 enables more oscillation in the system.

So with my line joint, a simple box hanging down, I set the frequency to 2.0 and damping to 1.0 and this works fine.

What do you think of this sort of behaviour?

ALso, one question about a cloth simulation. I have set the vertical and horizontal distance joints to very stiff (100Hz) and overdamped at 10 as these are shear and bending joints, and the diagonal joints to mildly stiff at 3Hz and underdamped 0.3 . The distance between each particle is 0.2m. You can see that the cloth is not smooth when adding a horizontal wind force. Are there any other constraints required?
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

I tested Box2D lite by removing warm starting and multiplying gamma by the inverse timestep and reproduced the same behaviour. However if you do not divide gamma by the timestep, the bridge behaves correctly

So now in Box2D lite, we have (no warm starting):
(JM^-1JT*gamma)*lambda = -beta/h*C

where gamma = 1/(d+kh) and beta = hk / (d+hk)

I don't really understand how this came about, as we derived the magic formulas for forces, but it still works for some reason.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Soft constraint derivation

Post by Dirk Gregorius »

So you are seeing problems with warmstarting the bridge with Baumgarte stabilization. Baumgarte and warmstarting do not working well together. Especially for closed loops. So this is actually normal. Personally I use position projection since it gives much better results. You can only warmstart with say 95%, but this makes the constraints soft. I recommend to try position correction. With Baumgarte you tweak yourself to death in my experience :D
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

Well since I have applied the Baumgarte technique on my other constraints with success, I have reshuffled some calculations for my revolute joint and now I am getting a really soft constraint rather than instability.

The revolute joint is tricky in 3D because the mass seen by the impulse JM^-1JT is not a scalar, therefore the spring constant and damping coefficient are 3x3 matrices

K = A * omega * omega
D = 2 * A * omega * zeta

I read in the ODE doc that the CFM updates the diagonal of the A matrix, so I'll try to figure out where I've gone wrong in my previous code that I pasted.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Soft constraint derivation

Post by c0der »

In Box2D Lite, the mass of each plank is used to calculate the spring stiffness and damping coefficient. I thought the mass of the constraint system is supposed to be used in order to incorporate gamma into the solver equation.

(JM^-1JT + gamma/h) * LambdaImpulse = -Jvi - beta/h*C

So if gamma wasn't a matrix of the same size of JM^-1JT, matrix addition would not be valid. From the behaviour of my line joint and distance joints, which need a lot less spring stiffness and damping, I think it is fair to assume that with a suspension bridge of large span, you would need a huge spring stiffness with a large damper to stop the oscillation.

Since Erin uses the mass of the planks to compute k and d, he would achieve a large k value and damping constant, even though it's not correct in the derivation. Am I looking at this correctly?
Post Reply