ODE contact model discussion in ODE mail list

Please don't post Bullet support questions here, use the above forums instead.
ngbinh
Posts: 117
Joined: Fri Aug 12, 2005 3:47 pm
Location: Newyork, USA

ODE contact model discussion in ODE mail list

Post by ngbinh »

I think it's good to bring here:
Hi All,
on ODE manual, it says, "ODE has hard contacts. This means that a special non-penetration constraint is used whenever two bodies collide."
But in my simulation example, which basicly was a ball rolling on ground, somehow, I read out the contact point position and found the Z - direction (vertical)
coordinates were -0.005. Which means the contact point was underground. How did it happened? Since ODE has Hard contacts for each collision.
Any way to avoid such penetration? Thanks.

Ruby
Hi!
"hard" here means the constraint will exist in the whole time step. In ODE case, we:
1. Set up the Jacobi correspond to the constraint
2. Solve for the constraint force fc

Then because the constraint is "hard" so we will apply fc to the involved bodies through the whole timestep. That's actually not "exact" because the contact might be broken in-between the time step.
This problem is inherent to any force-acceleration level simulation and along with friction, there is singularity problem also.

If you change the constraint system into velocity-impulse level then you don't have "hard" or "soft" contact anymore because you treat all of them as impulses.

Binh Nguyen
Nguyen,

I would argue that the ODE solves on the velocity level. Basically you have an DAE (for simplicity for holonom constraints):

dv/dt = W * ( F_ext + JT * lambda )
dx/dt = v

C(p,q) = 0

We use the differential quotient for the accelerations:

dv/dt ~= ( v(t+dt) - v(t) ) / dt -> v(t+dt) = v(t) + W * ( F_ext + JT * lambda ) * dt

We can find the velocity constraints through differentiation of the position constraints

dC/dt = dC/dx * dx/dt = J * v(t) = 0

Since we use the velocities of the end of the timestep (Symplectic Euler) we can require J * v(t+dt) = 0

If you combine these equations you end up with the well know LS (or MLCP in the presence of non-holonom constraints)

J*W*JT*lambda = -( v(t)/dt + W * f_ext )

You could multiply the whole equation by dt > 0 and get

J*W*JT*(lambda*dt) = -( v(t) + W * f_ext * dt )

You clearly see the forces and impulses are linear related in this model. Also I argue that in every discrete model a constraint can't break. Of course a constraint force could become zero (through clamping), but the ODE can handle this as well.



So regarding the original question. Because of the discrete collision model you can end up in penetration. This has nothing todo with hard or soft constraints. The ERP parameter basically handles how much of the error is corrected each timestep. Also note that ODE has some value that allows for some penetration before the error correction jumps in. This is in order to deal with jitter and to improve coherence. So the 0.005 you see might be the default value for the allowed penetration.

HTH,
Dirk Gregorius
A hard constraint basically means that you have a "hard" algebraic
constraint equation instead of plugging in a "soft" spring like in a
penalty method. The ODE requires for each contact:

1) The penetration at the contact should vanish
C(p,q) = (p1 + r1 - p2 - r2) * n - d = 0

2) The relative velocity should vanish
dC/dt = (v1 + cross( omega1, r1) - v2 - cross(omega2, r2) * n

-> J = ( n, cross( r1, n ), -n, -cross( r2, n ) )

Since we apply only forces when we have an approaching relative velocity
at the contact we basically end up with

w = A*x + b >= 0 and x >= 0 and xT * w = 0 which is a classic LCP or
MLCP in presents of additonal holonome constraints.


Actually each position constraint imposes also a velocity- and
acceleration constraint onto the system. Think of a ball socket joint.
The position constraint requires that the anchors are at the same point
in space. In order to maintain this constraint the relative velocity and
acceleration must vanish at the anchor. Otherwise the anchors would get
apart. So *all* constraints (position, velocity and acceleration) must
always be satisfied. The ODE solves only at the velocity level. Note
that even when you fully satisfied the velocity constraints, you still
can end up with violated position constraints (e.g. because of high
angular velocities). The ODE deals with this using Baumgarte
stabilization. Erin Catto's has a nice presentaion about this at
http://www.gphysics.com.

Dirk Gregorius

But ODE still solve for constraint forces. That makes me refer to ODE as acceleration-force model.
The equations you showed are basically discretized version of Newton-Euler equation. And yes, ODE use velocity as variables there.


Because ODE solve for the contact forces so the constraints will be satisfied at velocity level.
But in a velocity-impulse contact model will solve for contact impulses so it will maintain the constraints *exactly* at position level and you don't need Baumgarte stabilization which adds stiffness to the system.
Nguyen Binh
How can this be?

Think of a pendulum. Your velocity constraint requires that the realtive
velocity is tangential to the pendulum. Now we satisfy this (velocity)
constraint. If you integrate now forward using the constraint satisfying
velocities you will by no means meet the position constraint. You will
end up outside the circle described by the pendulum

So I assume I misunderstand you. Can you please give an example how you
find impulses to project on the velocity constraint manifold and then
after some update of the positions I also atomatically satisfy the
position constraint?

Here is an example:
In an impulse solver (equivalent to the ODE) you would solve as follows:

C = |p2 - p1| - L0
dC/dt = (p2 - p1) / |p2 - p1| * (v2 - v1) -> J = ( -(p2 - p1) / |p2 -
p1| (p2 - p1) / |p2 - p1| )

1) Update velocities
2) Apply iteratively sequential impulses until all velocity constraints
are saisfied

lambda = - J*v / J*W*JT

P1 = -(p2 - p1) / |p2 - p1| * lambda
P2 = -P1 = (p2 - p1) / |p2 - p1| * lambda

v1 += P1 / m1
v2 += P2 / m2

3) Update positions using new velocities


How would this look in your model?
Dirk Gregorius
Good catch!
You are right about the fact that it will end up outside the circle. But that's because we linearized the position constraint. In the pendulum case, we have C(q) = q^2 - r^2 = 0 but we only enforce it in velocity level.
That's very bad if you use acceleration-force model because the end point will move away from original circle every time step. With velocity-impulse, the end point will move in a sets of line that circumscribed the circle, which is not so bad.

Velocity-Impulse:
+Bilateral constraint: C(q) = 0

+Taylor series expansion: C(q)[t+1] = C(q)[t] + dC/dq*delta(q) + dq/dt*h = 0; (1)
h is time step, C(q)[t+1] means the value of C(q) at time t+1.
Note that we only use the linear terms, you can use more but it will make your system non-linear.

+ Divide by h to have: C(q)[t]/h + J*v[t+1] + delta(q) = 0; (2)
Here the variable is v[t+1], means we need to find impulses such that v[t+1] satisfy (2).
If you apply this to the pendulum case, you can see that the end point will never sway too far from the circle. And if you decided expand Taylor series in (1) then it will satisfy C(q) exactly.

That's the constraint we need to satisfy. You can see that it's not exactly the same as your constraint.

All of this method has been discussed quite alot in papers by Jeff Trinkle. Anitescu, Potra.

Nguyen Binh
Erin Catto
Posts: 316
Joined: Fri Jul 01, 2005 5:29 am
Location: Irvine

Post by Erin Catto »

+Taylor series expansion: C(q)[t+1] = C(q)[t] + dC/dq*delta(q) + dq/dt*h = 0; (1)
h is time step, C(q)[t+1] means the value of C(q) at time t+1.
Note that we only use the linear terms, you can use more but it will make your system non-linear
I don't understand your Taylor expansion. Here's my derivation for small h:

C[t+h] = C[t] + dC/dt * h

dC/dt = J[t] * v[t] + partial_C[t] / partial_dt

So the velocity constaint is:

J * v = -((1/h) * C + partial_C / partial_dt)

C represents the old position constraint and partial_C / partial_t represents a specified motor velocity.

Well a velocity constraint with Baumgarte stabilization is just:

J * v = -((beta/h) * C[t] + partial_C / partial_dt)

So what you have is Baumgarte stabilization with a factor of 1! I usually don't recommend such a large Baumgarte factor because it adds energy to the system.

As I recall the Stewart & Trinkle integrator deals with the constraint by eliminating the velocity with the position and then solves for the position update. This reduces the stability problems of using beta=1, but you need some extra constraints to deal with friction (which is a nonholonomic constraint). Is this correct?
ngbinh
Posts: 117
Joined: Fri Aug 12, 2005 3:47 pm
Location: Newyork, USA

Post by ngbinh »

Erin Catto wrote:
I don't understand your Taylor expansion. Here's my derivation for small h:

C[t+h] = C[t] + dC/dt * h

dC/dt = J[t] * v[t] + partial_C[t] / partial_dt

So the velocity constaint is:

J * v = -((1/h) * C + partial_C / partial_dt)
C is a function of q (generalized position and time) so you have to take derivative of C as a 2 variables function.

And also, we don't have to take derivative of C(q) to represent the constraint in velocity level.
Erin Catto wrote: C represents the old position constraint and partial_C / partial_t represents a specified motor velocity.

Well a velocity constraint with Baumgarte stabilization is just:

J * v = -((beta/h) * C[t] + partial_C / partial_dt)

So what you have is Baumgarte stabilization with a factor of 1! I usually don't recommend such a large Baumgarte factor because it adds energy to the system.
The C[t] term can be viewed as stabilization term but it's the result of Taylor series expansion not "artificial" stabilization factor so it won't add energy to the system. You can be sure about this because what we really care about is accuracy and we did alot of analysis on this issue. You can refer to our group papers to see.
Erin Catto wrote: As I recall the Stewart & Trinkle integrator deals with the constraint by eliminating the velocity with the position and then solves for the position update. This reduces the stability problems of using beta=1, but you need some extra constraints to deal with friction (which is a nonholonomic constraint). Is this correct?
The major contributions of ST integrator are twofolds:
1. Time stepping: It view dynamical system as a series of time steps, thus it removed many problem with continuous method (mainly singularity). Also, this method is backed up by proofs that it will converge to continuous model given time step is small.
2. Velocity-Impulse : Instead of force-acceleration, it uses velocity and impulse. Thus it solved the problem raised by friction. In acceleration-force model, there are cases when the problems are unsolvable (Refer to David Stewart's papers)

About friction, Stewart-Trinkle is the first model that comply with Coulomb friction. I don't think current friction models using in games really comply with this model. And yes, friction makes ST model hard because it make the big matrix not symmetry.

Also, you can treat friction in Stewart-Trinkle in different ways to make it as easy as game's LCP:
1. You can assume that all normal forces are known, I think that's the way gaming people did. (Correct me if I'm wrong, I left gaming industry for 3 years now)
2. You can solve the system in 2 passes. First pass with no friction, 2nd pass just for friction (when normal force are known, it's just a QP). This is the usual method used by Dinesh Pai at Rutgers.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Post by Dirk Gregorius »

C is a function of q (generalized position and time) so you have to take derivative of C as a 2 variables function.
You mean C( q(t), t ), right? So what is the problem with Erin's derivative please?

And also, we don't have to take derivative of C(q) to represent the constraint in velocity level.
Are you sure? What are you doing instead?

The C[t] term can be viewed as stabilization term but it's the result of Taylor series expansion not "artificial" stabilization factor so it won't add energy to the system
I sees nothing artificial in Baumgarte stabilization. This is mathematically founded method
ngbinh
Posts: 117
Joined: Fri Aug 12, 2005 3:47 pm
Location: Newyork, USA

Post by ngbinh »

Dirk Gregorius wrote: You mean C( q(t), t ), right? So what is the problem with Erin's derivative please?
He treated as a function of h only.
Dirk Gregorius wrote: Are you sure? What are you doing instead?
I showed it in my quote before already.
Dirk Gregorius wrote: I sees nothing artificial in Baumgarte stabilization. This is mathematically founded method
In Baumgarte, you introduced an artificial term to stabilize a stiff system. It can introduce energy to the system.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Post by Dirk Gregorius »

He treated as a function of h only.
Nope, he didn't.

C( q(t), t )
dC/dt = dC/dq * dq/dt + partial_C / partial_t = J * v + partial_C / partial_t

I strongly doubt that there is anything wrong with this. Example:

C( q(t), t ) = (x2(t) - x1(t) )^2 + sin( 2*t )
dC/dt = 2 * ( (x2(t) - x1(t) ) ) * ( v2(t) - v1(t) ) + 0.5 * cos( 2*t )

So exactly what Erin wrote.

@Baumgarte:
You term is then totally artificial as well, since it results out of the linearization in the Taylor expansion.
ngbinh
Posts: 117
Joined: Fri Aug 12, 2005 3:47 pm
Location: Newyork, USA

Post by ngbinh »

Dirk Gregorius wrote:
He treated as a function of h only.
Nope, he didn't.

C( q(t), t )
dC/dt = dC/dq * dq/dt + partial_C / partial_t = J * v + partial_C / partial_t

I strongly doubt that there is anything wrong with this. Example:

C( q(t), t ) = (x2(t) - x1(t) )^2 + sin( 2*t )
dC/dt = 2 * ( (x2(t) - x1(t) ) ) * ( v2(t) - v1(t) ) + 0.5 * cos( 2*t )

So exactly what Erin wrote.

@Baumgarte:
You term is then totally artificial as well, since it results out of the linearization in the Taylor expansion.
I meant this part:

<quote=Erin>
C[t+h] = C[t] + dC/dt * h
</quote>

Actualy it shoud be:

C[t+h] = C[t] + dC/dq * delta(q) + dC/dt * h. (*)

From that we did not take derivative of C[t+h] but use (*) as our constraint equation. As you can see, we don't add anything in. You can also use the exact fully non-linear constraint if you want. But it will result in non-linear CP. Currently we also have some works on this fully non-linear system where we even mix collision detection and physics in one formulation thus remove *most* of the approximations. The paper on this work hasn't out yet so I can't publish here.
:D
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Post by Dirk Gregorius »

So you linearize the position constraint. I get this. But then you solve on the position level? I could see that you could use something like below for the Newton-Euler equation and plug it into your linearized constraint:

dv/dt ~ ( x(t+dt) - 2x(t) + x(t-1) ) / Sqr( dt ) = W * ( F_ext + JT * lambda )

So how do you use your linearied constraint and integrate your equations of motion?
Erin Catto
Posts: 316
Joined: Fri Jul 01, 2005 5:29 am
Location: Irvine

Post by Erin Catto »

Nguyen,

Please look at the definition of a total derivative:

http://en.wikipedia.org/wiki/Total_derivative

When I write dC/dt, I'm taking the total derivative. I then expanded the total derivative using partial derivatives:

dC(q(t),t)/dt = partial_C / partial_q * dq/dt + partial_C / partial_t

= J * v + partial_C / partial_t

Box2D (and ODE) are actually using a spin-off of Anitescu's velocity formulation. The modifications are:
1) less accurate friction models
2) less accurate solvers (iterative vs. pivoting).
3) Baumgarte stabilization.

Keep in mind that at some level, everything about our models is "artificial". Bodies are not perfectly rigid. The restitution and Coulomb friction are gross approximations.

Yes Baumgarte can add energy, but in some situations we are willing to live with it because of the considerable performance gains. I think that we should try to make our model's and solvers as accurate as possible given the performance requirements. So when someone comes along and offers an improvement to Baumgarte with just a small performance hit, I will take it.

BTW, to show you that I'm some what familiar with David Stewart's algorithm, you can download a simulator I wrote in '99 that:
- Uses Stewart's position based formulation.
- Uses a Lemke pivoting solver.
- Uses a variation of Featherstone's algorithm.

http://www.gphysics.com/files/OldDemos.zip
ngbinh
Posts: 117
Joined: Fri Aug 12, 2005 3:47 pm
Location: Newyork, USA

Post by ngbinh »

This is a quite rigorous treatment about the method:
http://www.cs.rpi.edu/~trink/Papers/Tidetc03.pdf
Take a closer look at section 3, you'll see how to use the constraint function without having to take derivation.

@Erin: your derivation is right. I only tried to answer your question about my Taylor series expansion. And about the differences between ODE's style and our method, what you've pointed out are correct but what I've been trying to show so far is we also use different constraint function. You can see in the paper that we don't take derivative of C(q) but used a approximation (linearization using Taylor series expansion) of constraint function instead. Hope that I'm clear enough.
Erin Catto
Posts: 316
Joined: Fri Jul 01, 2005 5:29 am
Location: Irvine

Post by Erin Catto »

Nguyen,

It would be interesting to compare a Baumgarte stabilized model with the ST model. Here is the Sequential Impulse algorithm with Baumgarte stabilization adapted to solve a single particle attached to ground by a distance constraint:

Integration of external forces:
v2' = v1 + h * g

Constraint:
C = 1/2 norm(x)
J = u where u = x/norm(x)

Setup constraint impulse with Baumgarte:
v2 = v2' + lambda * u
u * v2 + beta/h * (norm(x) - L) = 0

Solve for constraint impulse:
u*(v2' + lambda*u) = -beta/h * (norm(x) - L)
lambda = -u*v2' - beta/h * (norm(x) - L)

Apply impulse:
v2 = v2' + lambda * u

Integrate position:
x2 = x1 + h * v2
ngbinh
Posts: 117
Joined: Fri Aug 12, 2005 3:47 pm
Location: Newyork, USA

Post by ngbinh »

What you mean in comparing two methods? Possibly, in equilateral constraints, they are somewhat similar but I think the strength of ST is its ability to handle Coulomb friction model(ofcourse with the cost of more computing power).

Right now, I have a preliminary version of ST incorporate in Bullet. I will release it soon. But there is one problem: ST requires some special requirements in collision response and right now Bullet isn't satisfied them. I think I will need to work with Erwin more to address it.
Erin Catto
Posts: 316
Joined: Fri Jul 01, 2005 5:29 am
Location: Irvine

Post by Erin Catto »

We have been discussing position stabilization and it would be interesting to see how ST handles that in this particular case. This would help to give us all insight into how ST can handle constraint error without Baumgarte stabilization.