Unstable rotational limits

[smaker]

Using iterative LCP-solver, I get wrong rotational limits in very special case: when I'm trying to limit rotations about one axis between two bodies. Test scene: ragdoll, which have capped cylinder as body and smaller one for thighs. The're aligned along Y-axis, but there is some translations between them along X-axis (i.e. body have center for example vector3(0, 1, 0) and thigs vector3(-0.5, -1, 0) and vector3(0.5, -1, 0)) and in that case there is some jittering during simulation, proportional to stabilization parameter of rotational limit. When a rotational limit set to a hinge joint for free rotational axis, it works perfect, and in a whole look my solver handles general joints quite stable. Where can be my mistake, or it should work like this with these little translations? And if so, how can I restrict that kind of rotations in ragdoll configuration?

[Dirk Gregorius]

What Jacobian do you use and how do you compute the angle? Do you use warmstarting?

[smaker]

Jakobian used for rotational limit:
linear component of Jacobian is set to zero
angular component is set as
- first string: -(Axis of Restricted Rotation);
- second string: Axis of Restricted Rotation;
this axes is different for each body (i.e. i store local axes for each body and the for body1's Jacobian use Body1->Rotation * localAxis and similar for body2)
right hand side for first string is 1 / dt * <stabilization parameter> * (lowAngleLimit - relativeAngle), for second string is 1 / dt * <stabilization parameter> * (relativeAngle * highAngleLimit);
Relative Angle is recieved from Relative Quaternion's dot product of vector component (something like in ODE);
lower limits for lambda is set to 0.0f for both strings and higher limits is set to FLT_MAX (I use single-precision mathematics);
warmstarting is used for contact joints only, because for another joints it shows not good enough.

[smaker]

RHS for second string is 1 / dt * <stabilization parameter> * (relativeAngle - highAngleLimit), in code that was right. Problem stays.

[Dirk Gregorius]

Difficult to say why you have problems. Basically limits are like contacts, so in order to avoid jitter you usually use some slop/allowed violation (e.g. Box2D slides). Do you do this already? I would also use a small Baumgarte parameter (e.g. 0.1 - 0.2). Finally if the limits are to soft you can tweak the inertia tensor a little by scaling it artificially (e.g. a factor of 4 - 32). Note that this results in stiffer behaviour, but might look unrealistic. So handle with care...

[smaker]

Ok, thanks for suggestions, I'll try to play with stabilization and stiffness parameters, and I'll try to test limits in variuous test scenes and then mb I'll find error or something..

[smaker]

Well, I've tested these damn limits.
In case of limited revolution joint it's all ok. Now, when calculations fail:
Sphere have position vector3(0, 3, 0) and another sphere have position (0, 1, 0) [note, there's no offset in x-, z- axes between bodies]; both spheres have same radius of 0.5 and idential mass; and they're connected via limited ball-socket so this construction is resting on the floor. Whe force is applied to rotate upper sphere in this situation, rotational limit works fine. But when Force is applied to upper body that causes this construction to fall on the floor (for example, vector3(10, 0, 0) force to the centre of mass of upper body), jittering is happened. I assume that something wrong with my relative quaternions calculus method. Have you good references on that (in ODE is _very_ lowlevel mathematics so i can't understand what's happening there; please do not send me there ), or at least I hope this descripton made things clearer and it will be easier for you to localize problem..
P.S. Playing with stiffness parameters, etc. had no results; jittering was even when I set iterations of solver to 150; while other joints stays stable even with 5 iterations, so I think that problem is in Jacobian calculations or something, but not in solver. If you have good references on this theme (exactly rotational limits), please post it here.

[Dirk Gregorius]

So you have a ball socket joint and how many axes are limited? I dig out the formula for the limit and post it here in a second...

[Dirk Gregorius]

For an angular limits around an axis w use:
C = theta - theta_min >= 0
C= theta_max - theta >= 0

The choice of constraint depends on which limit is being violated.
When the constraint is switched, lambda should be set to zero for
warmstarting (if you want to use it again later).

Theta is computed using this:
theta = atan2( dot( u2, v1 ), dot( u2, u1 ) )

Amazingly the true Jacobian is simple:
dC/dt = dot( omega2 - omega1, w1 ) ===> J = ( 0 | -w1 | 0 | w1 )


At the anchor we define an orthonormal basis u,v,w where w is the limited axis and in
order for the angle computation to work as expected w = cross(u, v) must hold otherwise you
get wrong results. So be carefull when you construct the basis from the limited axis. Don't use
dPlaneSpace() from the ODE...!

On construction you save the basis local in each body and when you test the limit you transform them
back to world coordinates. So in the above formulas u1 is the global axis from the first body, e.g.

u1 = Vec3TransformTo World( R1, local_u1 ); // R1 is the current rotation matrix...


Can you implement this?


HTH,
-Dirk

[smaker]

First of all, thanks for so wide answer.
Well, i ve located error in my scheme. That was wrong calculation of initial relative rotation of two bodies, now it's all OK except that i need to divide RHS by some magic number (if I do like before, 1 / dt * <stabilization parameter> * (Angle - lowAngle) if the lower limit is violated, there is to strong rotations of bodies that made them maintain the constraint). I divide by about 50.0 at current time. I'll try your scheme with storing ortho-normal basis for each body that is created by restricted axis, but I've got some questions: how can I create that basis if I had only one vector? I've only various cheats in my mind, for example, find first axis by cross product of W and some random axis (e.g (1, 0, 0) and if W is close to it, I'll take (0, 1, 0)) and second axis with cross product of these two..
PlaneSpace in OpenDE is some kind of hardcore hack, and I'm afraid of using it

[smaker]

Ok, thanks for mentioned here solution with storing basis; I've implemented it recently and in test configuration this approach was veery handy Looks like something wrong was with my quaternion math, since after testing a bit that I called working version with a magic number, I've found it quite unstable. Now, with your version it's almost OK. _Almost_.. As I said before, it's very good on test configurations; but when I've launched 20 ragdoll demo, there was still some jittering (and in some cases jittering was very hard, bodies was dancing until all limits correspond; those cases was not numerous, but anyway, looked not so plausible).

[Dirk Gregorius]

Does the jitter gets away when you increase the iteration count?

To create the orthonormal basis from the locked axis I proceed like this:

Code: Select all

void AngularLimit::Create( Vec3 axis, float lo, float hi )
{
Vec3 w = axis;

Vec3 u = Vec3GetUnitPerpendicular( w );   // Make sure the axis is normalized!
Vec3 v = Vec3Cross( v, w, u );
assert( Vec3FuzzyEqual( w, Vec3Cross( u, v ) ) );

mLocalU1 = mpBody1->TransformToLocalSpace( u );
mLocalV1 = mpBody1->TransformToLocalSpace( v );
// ...

mLocalU2 = mpBody2->TransformToLocalSpace( u );
// ...
}

In order to find the correct angle you do:

Code: Select all

float AngularLimit::GetAngle( void ) const
{
Vec3 u1 = mpBody1->TransformToWorldSpace( mLocalU1 );
Vec3 u2 = mpBody2->TransformToWorldSpace( mLocalU2 );

Vec3 v1 = mpBody1->TransformToWorldSpace( mLocalV1 );
	
return Math::Atan2( Vec3Dot( u2, v1 ), Vec3Dot( u2, u1 ) );
}

Make sure you have the transformations right! There is a good article on the in GPG I.

[smaker]

Jittering is not depending from iterations at all.. Seems like it's too strong forces trying to push bodies into limits, it's actually correct almost every time, but suddenly some of ragdoll's limb starts to "dance" for several moments and then calms down when all limits corresponds. Very awkward behaviour, I must say..
That's how i compute and store vectors:

Code: Select all

vector3 U, V, W;
W = _RestrAxis.Normalize();
if (fabsf(W.x - 0.7f) < 0.1f)
	U = (vector3(0, 1, 1) * W).Normalize();
else
	U = (vector3(1, 0, 1) * W).Normalize();
V = (W * U).Normalize();

U1 = Body1->R_1 * U;
V1 = Body1->R_1 * V;
W1 = Body1->R_1 * W;
if (Body2)
{
	U2 = Body2->R_1 * U;
	V2 = Body2->R_1 * V;
	W2 = Body2->R_1 * W;
} else
{
	U2 = U;
	V2 = V;
	W2 = W;
}

where _RestrAxis is restricted axis, and Body#->R_1 is inversed rotational matrix of body # (transposed, actually, but it's equal for matrices that store only rotations). Between vectors, '*' is a cross product.
And that's how I compute angle:

Code: Select all

// Returns:
//	-1 - lower limit is violated
//	 0 - all is OK
//	 1 - upper limit is violated
int testLimits()
{
	vector3 wsU1(Body1->R * U1), wsV1(Body1->R * V1), wsW1(Body1->R * W1);
	vector3 wsU2(U2), wsV2(V2), wsW2(W2);
	if (Body2)
	{
		wsU2 = Body2->R * wsU2;
		wsV2 = Body2->R * wsV2;
		wsW2 = Body2->R * wsW2;
	}

	Ang = -atan2f(wsU2 % wsV1, wsU2 % wsU1);

	if		(Ang < loAng) testResult = -1;
	else if	(Ang > hiAng) testResult =  1;
	else testResult = 0;

	return testResult;
}

R is rotational matrix of body, note that i need to take atan2 with a different sign, because if I'll take it positive, values of angle is wrong. Priniting in log shows that basis is calculated well (W == U x V) and Angle seems quite natural..

P.S. '%' with two vector operands is dot product.

[Dirk Gregorius]

The angle is not negative! Do you use the right hand rule to identify positive and negative rotations?

How did you implement your iterative solver? PGS (projected Gauss-Seidel) like in the ODE or SI (sequential impulses) like Catto? They are equivalent mathematically though, but differ in the implementation...


Cheers,
-Dirk

[Dirk Gregorius]

Can you please try this to find an orthonormal vector (I haven't checked your code, so it might be they are equivalent):

Code: Select all

Vec3 out;

// v is the passed vector
float ax = abs(v.x);
float ay = abs(v.y);
float az = abs(v.z);

if ( ax < ay )
   {
	if ( ax < az )
		{
		// ZeroX:
		Vec3Set( out, 0.0f, -v.z, v.y );
		}
	else
      {
      // ZeroZ:
		Vec3Set( out, -v.y, v.x, 0.0f );
      }
	}
else
	{
	if ( ay < az )
		{
		// ZeroY:
		Vec3Set( out, -v.z, 0.0f, v.x );
		}
	else
		{
		// ZeroZ:
		Vec3Set( out, -v.y, v.x, 0.0f );
		}
  }