Refactoring joints

[Julio Jerez]

Ha you got at the end. Sleep should never be part of the simulation. Sleep is a post effect that only apply after the island reach equilibrium, not before.

Yes a kinematic body requires some penetration to reach equilibrium.
This is because on the right side of the equation 10 we treat them as infinite mass, so only the force acting in the bodies in contact with it is considered to calculate the contact acceleration therefore in the abcent of penetration penalty, they will slowly keep penetrating.
When the penetration penalty is present, then there is dynamic effect that as the body penetrates the kinematic body, penetration increases unlit it matches the acceleration produced by the weight.

You can see this in the class contact.cpp
I am giving a very simple explanation here but the class implement a more sofisticated method, where it calculates the amount of acceleration considering, restitution, penetration stiffness and penetration.
The example was to show why a kinematic body can't be either static of very small mass.

The problem with joint seems different, can you change the sandbox demo to reproduce what you are getting adding joints?

I committed the test.

[Julio Jerez]

In the end the reaction force will be

L = (-g * m0 + k* h)/ (m0 + m1)

m0 is the mass of dynamic body
m1 is the mass of kinematic body
k penetration stiffness
h penetration

You can see that the numerator when k is zero then the contact acceleration can never be zero.
For the numerator to be zero k has to be non zero and h has to be negative, so it cancel the acceleration produced be the weight.

In the denominator you can see ta the mass of the kinematic body act as a damper that reduces the total value of the numerator, since it is always a positive value. A small value generate large acceleration.

Adding huge large mass does not do anything.
If you plug a huge value of m1, what it does is that the whole this will be a small number not matter what the other parameter are.
But it is much worse because in this simple case of one dimension we can calculate the denomination as the inverse of the masses, but on complex problem the denominator is in fact the inverse of a matrix and that bring a whole set of new numerical problems with condition number, so not do not plug ridiculous values, it will not make better and chance is that it will make much worse.

[JoeJ]

Makes sense. I start to think it is something on my end. I just placed a 10 bodies joes ragdoll tower on top of the kinematic body, but no issues even with very low kinematic mass.
There are other issues with my ragdoll as well (fails to balance), so i'll remove platforms for now, fix things and try again later...

[Julio Jerez]

I will continue completing the joints, see if I get the same problem.
for now every thing seems to go right.
There are few demos that should expose that problem once all the joints are converted to be based of the 6dof.

[Julio Jerez]

Ok Joe I am back again on the 3dof I am using a slightly different approach and IO committed the test.
when you have time, can you please sync and test it, tell me is this is intuitive to you now.
we need more general test at least this test seem to work as I expect it would.

I will write the derivation later, but I has done this in many occasion I the pass and I always had problem with it until now. and I believe the reason was fail in the pass was because I did no have the Euler derivation in the joint instead it was added as external torque.

anyway please see if that makes sense to you when you get some time to sync.

edit:
to see the difference there is a #if 1
in file ../sdk\dCustomJoints\dCustom6dof.cpp line 350

you can change to 0 and this will do the old method, and you will see that it manage to fail in critical orientations. while the current method seem to do better.

[Julio Jerez]

never mind still blew up as soon as I added two links.

I nee to write a repro so that I can see where this is happing, this seem a fundamental error some where that is skipping me. and seem each time is different I can reproduce it.

so I will do that in the next day or two. now that we use immediate mode it should be easier.

[Julio Jerez]

oh I has a bug, the axis were swapped in the demo, it does no mean is right by is no ass bad as what I saw first.
anyway see if this is better, when you get some time please.

I still nee to add that replay stuff so that is easier tor debug these stuff

[JoeJ]

tell me is this is intuitive to you now.

Hm, what i intuitively think is that this can never work - but that's based on assumptions.
You need to tell me:
How does the order you provide rows finally matter in the solver
I do not know if they get solved 'at once' or in given / random order.

I think if you want a joint like this, you allways end up with the requirement to use multiple bodies, because in reality you would need multiple joints as well to make this complex mechanically restricted joint. (But it might work if the solver solves row in order?)

[Julio Jerez]

How does the order you provide rows finally matter in the solver
I do not know if they get solved 'at once' or in given / random order.

ah that's the trick, the point is the order of the angle does not matter, as long as the angles are small.
That's the Rodrigues theorem, is hard to find the proof anywhere, I have in a book Dynamics of Multibody Systems by Ahmed A, Shabana page 42.
it represents the rotation as complex number by expanding the sin and cos in using Taylor
leading to a expansion of the for I + v * sin A + 2 V^2 sin2(A/2)
the product of these expansion shows that anything beyond the third term can be neglected.
then the product of these three terms index shows that angular rotation do not commute.
because of the effect of the imaginary part,
then on page 49, it shows that for infinitesimal angular rotations, the third term of the expansion is quadratic and can also be neglected and the product of these two expression (I + v sin (A)) * (I + v sin(B)) do commute. It is an interesting there but is a long, long time I do not read those book,
I just know the theorem was in one of my books.
but I found a segment in Wikipedia that does a simple demonstration.
https://en.wikipedia.org/wiki/Rotation_group_SO(3)#Infinitesimal_rotations

They do not proof the theorem they do a verification to show that in fact concatenating small angular rotation, the rotation matrices commuted on the first order term plus there is a second order error term that vanish for really small angles. (the definition of small angle is the part I am abusing).
here is the conclusion:

Since dθ dφ is second order, we discard it: thus, to first order, multiplication of infinitesimal rotation matrices is commutative. In fact,
d A x d A y = d A y d A x
again to first order. In other words, the order in which infinitesimal rotations are applied is irrelevant.

I am abusing the concept, because when they talk about infinitesimal angles the really mean small angles and I am declaring the error angle small regardless of their magnitude, therefore my second order error is not so negligible.
But that is my claim, I claim the we can manage the error even for relative small error angles like under 5 to 10 degrees.

The Theorem is 100% true, in fact Quaterion rotation is a Corollary of the theorem.
It is I who is stretching to the breaking point where it fail.
However no matter how good the theory is, it is not working so is back to the drawing board and see if I can find some error on my side.

I will later write a agebraic demostartion the in fact the order of the angles is irrelevant in the newton system, maybe I find my mistake.

This is very important for what I want to do, because quaternion is not going to do it. a quat assume the the rotation is predetermined so it can only do a motors along the quaternion axis, but the axis is not known.

[Julio Jerez]

one reason I still have hope that there is a bug some where is that
as along as is no hitting the limits it seems to behave right, even when it his the limits
but the some how for no reason and angle jump lie crazy
here is a listing of the joint angle for one joint

-32.445980 0.000000 0.000000
-19.017563 0.000000 0.000000
5.322206 0.000000 0.000000
79.888283 0.000000 0.000000

you can see taht a pithc angle jump form -19 to 5 and them back to 80, this is a sign that the must be a a coding bug other than the theory.

this is why I need to add code to reproduce these sequences reliably.

[JoeJ]

Ah, remember and makes sense. I do not understand this (would need to do some coding and visualization), but i imagine it is like doing multiple skews and normalizing the result to get a rotation.

In my ragdoll joint i use the error axis instead acceleration axis for the rows, which causes a bit of rotation going to the two stabilization axis as well. But Newton tolerates this and the result is better than when using acceleration directly and having zero acc on the other two axis.
Another indication there is hope for your approach

Often i really think our knowledge about 3D rotations is still incomplete. AFAIK we do not know a way to get a weighted average of 3 rotations for example. Recently i worked on the generation of crossfields on mesh surface and i represented the cross directions with sine waves, so i can add weighted waves to get a weighted average due to their periodic nature. This hinted me that maybe using spherical harmonics could do the same in 3D, eleminating the problem of flipping angles and related opposite rotation axis directions. Maybe that could be of use here somehow, but i guess that's just crazy thoughts... will think of it...

[Julio Jerez]

well I added a replay for mouse pick, so sure if it reproduce the replay exactly because the has not check is the muse pick code still uses the input system directly.

but it seem to work, is in file ...\demosSandbox\sdkDemos\DemoCameraManager.cppline 130

tomonow I will test it an dem\bug what is going on.

Ialso foudn anoteh bug tha was no really a bug, but that now that teh joint us eteh full derivatove is become redundant, but that still does nothing.

it is almost as rotation in one axis are applied to a different one only in some orientations.
This is expectant if the angles are large, the bug is to determine why the angle is some orientation doe that huge jumps and that's what is hard to reproduced without replay.

anyway, that all I have thsi morning, late for work now.

[Julio Jerez]

I can now reproduce the same error. I can't debug now but I will check it out tonight
here is a history of the angles as the go out of control
f is the frame
j in the joint id and the three numbes are the joint angles pitch Yaw Roll in degrees

f:0 j:0 (0.000000 0.000000 0.000000)
f:1 j:1 (0.000000 0.000000 0.000000)
f:1 j:0 (0.000000 0.000000 0.000000)
f:2 j:1 (0.000000 0.000000 0.000000)
f:2 j:0 (0.000000 0.000000 0.000000)
f:3 j:1 (0.000000 0.000000 0.000000)
f:3 j:0 (0.000003 0.000000 0.000000)
f:4 j:1 (0.000076 0.000000 0.000000)
f:4 j:0 (-0.000010 0.000000 0.000000)
f:5 j:1 (0.000217 0.000000 0.000000)
f:5 j:0 (-0.000109 0.000000 0.000000)
f:6 j:1 (0.000080 0.000000 0.000000)
f:6 j:0 (-0.000329 0.000000 0.000000)
f:7 j:1 (-0.000565 0.000000 0.000000)
f:7 j:0 (-0.000570 0.000000 0.000000)
f:8 j:1 (-0.001376 0.000000 0.000000)
f:8 j:0 (-0.000642 0.000000 0.000000)
...
...
f:38 j:1 (-0.248424 0.000000 0.000000)
f:38 j:0 (-0.002646 0.000000 0.000000)
f:39 j:1 (-0.209899 0.000000 0.000000)
f:39 j:0 (-0.004551 0.000000 0.000000)
f:40 j:1 (-0.309613 0.000000 0.000000)
f:40 j:0 (0.002330 0.000000 0.000000)
f:41 j:1 (-0.297797 0.000000 0.000000)
f:41 j:0 (0.009137 0.000000 0.000000)
f:42 j:1 (-0.291879 0.000000 0.000000)
f:42 j:0 (0.011212 0.000000 0.000000)
f:43 j:1 (-0.216458 0.000000 0.000000)
f:43 j:0 (-0.001228 0.000000 0.000000)
f:44 j:1 (-0.131970 0.000000 0.000000)
...
...
f:75 j:1 (0.591350 0.000000 0.000000)
f:75 j:0 (-0.491203 0.000000 0.000000)
f:76 j:1 (1.253757 0.000000 0.000000)
f:76 j:0 (-0.473270 0.000000 0.000000)
f:77 j:1 (1.372265 0.000000 0.000000)
f:77 j:0 (-0.441242 0.000000 0.000000)
f:78 j:1 (1.958080 0.000000 0.000000)
f:78 j:0 (-0.454721 0.000000 0.000000)
f:79 j:1 (1.966522 0.000000 0.000000)
f:79 j:0 (-0.373938 0.000000 0.000000)
f:80 j:1 (3.080425 0.000000 0.000000)
...
...
f:249 j:1 (-0.012538 -0.139882 0.000000)
f:249 j:0 (1.038174 0.000000 0.000000)
f:250 j:1 (-0.022743 -0.145374 0.000000)
f:250 j:0 (1.461563 0.000000 0.000000)
f:251 j:1 (-0.031562 -0.153242 0.000000)
f:251 j:0 (1.720024 0.000000 0.000000)
f:252 j:1 (-0.035191 -0.158392 0.000000)
f:252 j:0 (1.631575 0.000000 0.000000)
f:253 j:1 (-0.032059 -0.162135 0.000000)
f:253 j:0 (1.078312 0.000000 0.000000)
f:254 j:1 (-0.022820 -0.164635 0.000000)
f:254 j:0 (0.048427 0.000000 0.000000)
f:255 j:1 (-0.009508 -0.167285 0.000000)
f:255 j:0 (-1.336115 0.000000 0.000000)
f:256 j:1 (0.005865 -0.172613 0.000000)
f:256 j:0 (-2.839605 0.000000 0.000000)
f:257 j:1 (0.021593 -0.183350 0.000000)
f:257 j:0 (-4.190651 0.000000 0.000000)
f:258 j:1 (0.035561 -0.201094 0.000000)
f:258 j:0 (-5.192595 0.000000 0.000000)
f:259 j:1 (0.045028 -0.225314 0.000000)
f:259 j:0 (-5.787219 0.000000 0.000000)
f:260 j:1 (0.047944 -0.252253 0.000000)
f:260 j:0 (-6.179031 0.000000 0.000000)
f:261 j:1 (0.045945 -0.277934 0.000000)
f:261 j:0 (-6.602828 0.000000 0.000000)
f:262 j:1 (0.043493 -0.304913 0.000000)
f:262 j:0 (-7.397254 0.000000 0.000000)
f:263 j:1 (0.043301 -0.325814 0.000000)
f:263 j:0 (-8.833097 0.000000 0.000000)
f:264 j:1 (0.047988 -0.335280 0.000000)

you can see that the pitch angle is stable at the beginning, the stat to oscillate, and that since a sign that something is wrong already but it is still an oscillation.
by then by the end you na see that the pitch angle keep becoming bigger and bigger as if the error add appositive feed back, that's a sign that some is wrong getting the angle of the rotation axis.
something seem to be flipping at some particular orientation.

[Julio Jerez]

just to make sure I am not debugging the wrong stuff, I am testing the straight cone limited joint. and that one since to behave quite nicely.

I will debug the there one tonight and if I do not find any a major bug, I will change the base 6dof joint to be cone limited.
No need to keep beating myself on a futile effort on something that we may have is a different solution.

The cool part of the cone limited is that, does not suffer from gimbal lock.
Also there is not implicit error angle since the rotations are aligned to the axis.
therefore larger angular step are possible.

Even the cone limited you can see the twist angle error can grow as large as 10 degrees but since it is aligned to the axis, it does show the weird behavior the euler of based joint.

I start to believe that I undertstatime the definition of infinitesimal angular rotation.
10 degrees angular error is not infinitesimal. when the say infinitesimal they really mean fraction of a degree.
The clue for this is that the error is second order so it does no vanish too easily, and I was under the impression that the error was the or fourth order.

Second order error neglecting means that that the quatiuoti that is evaluated must be really small.

[Julio Jerez]

joe I committed tow sample of the two different strategies.

the fix on teh join derivative using the wrong estimation of delta angle seem to made better, but was had to compared not having any reference so I added a cone for comparison.

The one of the left does angular limit the more or lest mimic the cone of the right
and both set twist angle set to zero.

I can not tell anymore which one is better, I am guessing the cone is but I am really bad at seen misbehavior visually.

imagine two two thing are an arm that the only has two dof, an arm really has tree.
The problem seem to come when the touch the limits.