Angular error with fixed joint

[John McCutchan]

hey all,

I'm continuing my adventures with implementing a SI constraint system. Anyways, I've introduced a fixed joint which fixes a single body so that they don't move or rotate.

I'm having a problem with stopping the rotation. I fix a block and then I drop another block on top of it's corner. The fixed block starts to rotate slowly until the dropped block slides off of it. But the fixed block never stops rotating...

Could someone verify my jacobian and baumgarte term. Kenny's thesis never explains how to compute Qerr but I think I figured it out from ODE's source.

My jacobian:

J = [I(6,6) 0(6,6)]

Where I is identity and O is zero.

I construct my baumgarte term as such:
(I'm going to skip the position entries)

on joint creation:
q_initial = bodyA->get_orientation().conjugate()

before applying impulses:
q_error = bodyA->get_orientation().conjugate() * q_initial.conjugate()
make q_error smallest angle
vector3 u = axis from q_error
u = bodyA->get_rotation_matrix() * u
b[3..6] = 2 * u

Thanks

[Dirk Gregorius]

Basically you setup the normal point-point constraint which removes 3 degrees of freedom and also a full orientation constraint. Setting up orientation constraints works as follows:

You define a constraint coordinate frame (u,v, w) which you store in each body. Bascially you want that u1 and u2, v1 and v2 and w1 and w2 are always algned. Instead of this we use the dot product and want the axes to remain orthogonal, e.g.

C1 = u1 * v2
C2 = u1 * w2
C3 = v1 * w2

Then you build the time derivate and find the Jacobian again by inspection, e.g.

dC1/dt = ( omega1 x u1 ) * v2 + u1 * ( omega2 x v2 )


The stabilization term is the integral of the velocity constraint and it is very hard to find it when you start with the velocity constraint. E.g. you could easily say that the relative angluar velocity must be zero and would get:

dC/dt = omega1 - omega2

But what is you stabilization? You could take the relative rotation of course and this might work (the ODE does this), but note that this would not be mathematically correct, because there is no differential relation between them then.

I suggest the following references (though both use the *not* so correct approach):

Thesis of K. Erleben
Thesis of M. Cline

You find a correct position based formulation in the thesis of Barthenbrug.


HTH,
-Dirk

[John McCutchan]

Thanks Dirk! You are always so helpful.

dC1/dt = ( omega1 x u1 ) * v2 + u1 * ( omega2 * v2 )

I assume you meant omega2 x v2.


I worked out the jacobian on paper and after a bunch of simplifying this is what I got:

Assuming that u1,v1,v2,w2 have been rotated into world space from their respective body space.

t1 = u1 x v2
t2 = u1 x w2
t3 = v1 x w2

The jacobians would then be:

J1A = [ t1, t2, t3 ]^T

J2A = [ -t1, -t2, -t3 ]^T

B1 = beta * u1.v2
B2 = beta * u1.w2
B3 = beta * v1.w2

I implemented this and it's working just as well as the old relative orientation method was.
So, if you see anything wrong with my derivation I'd appreciate your help. Could you point me
to Barthenbrug's thesis, I googled but couldn't find it.

Thanks

[Dirk Gregorius]

This looks correct.

The Jacobians schould have the following form:

J_linear = ( I3 -R1 -I3 R2 )
J_angular = ( 0 T 0 -T )


I3 is the identity and R1 ist the skew matrix for r1 (the lever arm from the COM to the anchor point) and R2 the same for body 2. T is the matrix holding you t_i computations in its rows. So I assume for beta you use 0.1 / dt and dt fixed at 60Hz.

My next question is how you relax the system. You have basically three options to solve the 6x6 system:

a) You solve each row individually ( this is basically PGS )
b) You solve the linear and angular constraint en block. This is solving two 3x3 system and this is what I would recommend
c) You solve the whole system directly what means to solve a 6x6 system.

Does you system explode or does it only move a little. In order to debug this stuff you can deactivate the stabilization (set it zero) and use a smaller timestep. Then also check the signs of your stabilization.

PM me your e-mail address and I send you the Barthenbrug thesis.


HTH,
-Dirk

[John McCutchan]

This looks correct.

The Jacobians schould have the following form:

J_linear = ( I3 -R1 -I3 R2 )
J_angular = ( 0 T 0 -T )

This is what I have. Except I'm fixing a single body at it's origin.
So R1, R2 are zero.

I3 is the identity and R1 ist the skew matrix for r1 (the lever arm from the COM to the anchor point) and R2 the same for body 2. T is the matrix holding you t_i computations in its rows. So I assume for beta you use 0.1 / dt and dt fixed at 60Hz.

My beta is 0.2 / dt and dt is fixed at 60hz.

My next question is how you relax the system. You have basically three options to solve the 6x6 system:

a) You solve each row individually ( this is basically PGS )
b) You solve the linear and angular constraint en block. This is solving two 3x3 system and this is what I would recommend
c) You solve the whole system directly what means to solve a 6x6 system.

I use c). I invert K once and use K^-1 for all future calculations.

Does you system explode or does it only move a little. In order to debug this stuff you can deactivate the stabilization (set it zero) and use a smaller timestep. Then also check the signs of your stabilization.

The system just moves a little bit, but it never stops. I've deactivated stabilization and switched to 120Hz and it is no better. The signs of my stabilization depend on which direction the body is starting to rotate in. So they vary between negative and positive.

Thanks for your help!

[John McCutchan]

Hey,

I have done some bug fixing and now the joint is working correctly with no stabalization.

With stabalization one of the axis settles (incorrectly) and then the joint spins around that axis for ever in a pulsing fashion.


Any suggestions?

Thaks.

[Dirk Gregorius]

Can you post some pseudo code of what you do?

[John McCutchan]

I'm just working on a single body scenario right now.

On setup:
RAI = bodyA->get_rotation_matrix_inverse ();
Roff = bodyA->get_position ();
uA = RAI * UNIT_X;
vA = RAI * UNIT_Y;
wA = RAI * UNIT_Z;
uB = UNIT_X;
vB = UNIT_Y;
wB = UNIT_Z;

Pre impulse:
RA = bodyA->get_rotation_matrix ()
uAW = RA * uA
vAW = RA * vA
wAW = RA * wa
uBW = uB
vBW = vB
wBW = wB

T1 = uAW.cross(vBW);
T2 = uAW.cross(wBW);
T3 = vAW.cross(wBW);

Code: Select all

J = [I | 0      | 0 | 0
     0 | T1,2,3 | 0 | 0 ]

T1,2,3 are laid out in rows
I and 0 are 3x3
K = J * W * JT
KI = K^-1

B[0..2] = Roff - bodyA->get_position()
B[3] = uAW.dot(vBW);
B[4] = uAW.dot(wBW);
B[5] = vAW.dot(wBW);

P = accumulated_impulse * J
apply P

Impulse:
v = [BodyA velocities | 0]

baum = beta * B - (J * v)

lambda = KI * baum
old_lambdas = accumulated_lambdas
accumulated_lambdas += lambda
lambda = accumulated_lambda - old_lambdas

P = lambda * J
apply P

[Dirk Gregorius]

I think you missed to transform the velocity:

v_rel = J * v

where

v = ( vA, wA, vB, wB ) = ( vA, wA, 0, 0 )

Read wA = omega of A

Let me know if this works then for you.

[John McCutchan]

I don't think I did. Take a look at the second line of the Impulse: section.

[John McCutchan]

I found the problem. It was in my 6x12 matrix class. I'm sorry to have wasted your time, it's working wonderfully now!

[Dirk Gregorius]

Cool!

Just two small points:

a) I am sure you know that, but you can also build the universal and hinge using the above formulas. For each axis you want to constaint find the tangent plane and require that the dot product becomes zero

b) In order to boost performance you should solve 1D and 3D linear and angular atoms. So for example for a universal you solve a 3D linear constraint (actually a point-point) and one angular constraint atom. Or for the fixed joint you can solve the 3D linear and angular atoms. This is faster than solving systems of higher dimensions with the same stability and can also be nicely implemented using SIMD


Cheers,
-Dirk