Contraint loop assert when body has large mass

[Esharc]

Good afternoon Julio,

I have a vehicle that has a very large mass. There are two legs attached to the body that help the vehicle move around when it needs to. I have set these legs up using 4 bodies and 4 hinges. This causes a loop of constraints.

BodyA would be my main body with the very large mass. To it are two hinge joints one connecting BodyB to BodyA and one connecting BodyC to BodyA. Then I have another two hinges connecting BodyB to BodyD and BodyC to BodyD.

I have a video here showing how they are connecting and the expected result.
https://youtu.be/V5a8cwV_jw4

The problem that I am having is that when I start updating the physics engine, I get an assert in ndSkeletonContainer.cpp InitLoopMassMatrix() at the end of the function where

Code: Select all

dAssert(dTestPSDmatrix(m_auxiliaryRowCount - m_blockSize, m_auxiliaryRowCount, &m_massMatrix11[m_auxiliaryRowCount * m_blockSize + m_blockSize]));

is called.

I traced the calling functions and in ndGeneralMatrix.h

Code: Select all

bool dCholeskyFactorizationAddRow(ndInt32, ndInt32 stride, ndInt32 n, T* const matrix, T* const invDiagonalOut)

there is a check

Code: Select all

 if (diag < T(1.0e-6f))
{
    return false;
}

I changed this check to T(1.0e-10f) just to get it to work so that I could create the video for you.
Is there anything that I am doing wrong here? Unfortunately the main body must have a large mass to keep it from toppling over since it has a long boom. It is a drag line.

Thank you in advance for any help

[Julio Jerez]

Few questions.
Are you using double precision?
In single precision make the tolerance so small does not help, because what happens, is that the matrix is I'll conditioned, and the factorization will produced two matrices that do not represent the original matrix.
This happen when a the inverse of a very heavy mass is added to the inverse of a very light mass.
If the number of significant digit is smaller than the dynamic range, the the result is just the light value.

What are the masses ratio?

Are you using newton 4.0
Newton 4 does a much better jub at this, by adaptive conditioning the matrix.

For what I can see in the video, the vehicle just sit on the floor. Is that correct?
I ask because,
Your description of the body arrangement form a kinematic loop that technically lead to a singular matrix. I believe that is the main problem.
What body D represents.

[Esharc]

Yes I am using double precision and I am using Newton 4.0.

Yes most of the time the vehicle just sits on the floor. But they walk the vehicle to different places when it has finished digging at one place. This is the purpose of BodyD. It is the leg that lifts the whole vehicle and moves it a very small distance. The ratio between the main body (BodyA) and the leg (BodyD) is 1500:1

Here is an example of what I am trying to achieve
https://www.youtube.com/watch?v=RP4Puhy ... IronPlanet

[Julio Jerez]

ok, double and newton4 check.

let us go over the schematic of the articulation that you describe. because I believe that where ether problem is.

fir let use show the simply schematic. of the real crane in the second video.
this is how it looks like to me, according to your simplifications.

Untitled1.png (3.53 KiB) Viewed 8345 times

this contraction should work, but it may require some tweaks.
the schematic shows the open loop, and the close loop will be formed when bodies D and F touch the floor.
when the hinge chain can deviate a little from plane of the hinge, that ok, because the penalties, which emulate the elasticity in this case should make the entire structure recover.
the reason is that any solve errors, is removed form ether system by the contact friction sliding, on the floor. In fact, that how the whole thing moves.

what you described and reiterated is this.

Untitled.png (5.75 KiB) Viewed 8345 times

here the joint are all hinges, and the will all move on the plane, this mean that in some space, the entire articulation is a tow degrees of Fredon articulation.
however because the mass ration are extremes, they error will be larges that when the penalties can deal with.
so what happen is that say the left side deviate a little, and the solve ad some correction, the right size will try to math that error, and will apply some correction, but that error does not disciple because there are not friction contact in the entire loop. therefore, in the next frame the solver will try to add new corrections and will continues on a positive loop. until the system explode.

first tell me if I have the schematic right, then we can try the corrections.

[Esharc]

With the first picture you are correct, and the walking of that dragline does seem simpler, although it could be deceiving. What I was describing with my first attempt was just a single side of the vehicle. So your second image that you posted need to be rotated 90 degrees.

DragLineWalkingBasicPicture.png (14.4 KiB) Viewed 8336 times

I have an exploded view of the walking mechanism here

DragLineLegExploded2.png (198.83 KiB) Viewed 8336 times

I also created a video showing the dragline with the two legs on both sides here
https://youtu.be/aHfEVPBeUn8

[Julio Jerez]

ah, I see.
Still each leg forms a four links kinematic loop.
those are solved all the time in mechanic without problems.
This is one of the advantages of generalized systems, since they never calculate the reaction forces and torque. but then again, generalized system have much bigger issues with contact and general non holonomic closed loops.

In reduced system, like Newton, the solver has to calculate the reaction forces to make the free body diagram and that's where the errors start to happen.
the errors will be larger, because of the large mass ratios, but that is not the problem, the problem is that the kinematic loop defines a plane. The solver tries to keep all links on that plane at any cost.
any slight deviation due to the errors on that matrix factorization, leads to unstable oscillations.

The solution is to allow for some slack on the plane so that the penalty for correction is dissipated.
this mean using a different joint.

please try this arrangement, and let me know, there are still another tweak you can try, to make more reliable.

Untitled.png (30.91 KiB) Viewed 8326 times

cylinder joints are just like hinges, but they allow for the spinning part to slide along the hinge pin.
on average the foot boot will not slide because the hinge on the other chain will not allow it to happen but when they do deviate from the plane doe to oscillation, there will be not positive feedback.

[Esharc]

Sorry for the delay in replying.

Unfortunately changing the joint to a cylinder joint did not solve the problem.

[Julio Jerez]

ok, here is what we can do.

1-you can try to make a repro test and send it to me so that I can see what is goind on.
this will be probably the best case.

alternatively, or you can try link the late file library to you project them export the file.
I believe is has enough to export the classes you are usings.

the advantage is that we can see a text version in xml form of the layout of the joints.

the steps are:
-Sync later.
-link libs ndFileFormat and ndModel

the to save a file you do this.

Code: Select all

ndFileFormatSave fileSave;
fileSave.SaveWorld(m_world, fileName);


to load the files, you do this

Code: Select all

ndFileFormatLoad fileLoad;
fileLoad.Load(fileName);
fileLoad.AddToWorld(m_world);


if you go this way, you can try loading the save file in the sandbox and see if it loads.
this seem more steps, but it is the solution. that I came with for these kinds of situations.
this way people can shred only xml export.

The file format does not save assets, only the collision shapes, so your asset are not compromised.

[Esharc]

Thank you for the reply Julio.

The good new is that I could replicate the issue in the Sandbox application. I sent you a PM with the code that you can use.

I attempting to use the save file format, but I see that you have made some changes since I last updated the engine on our side. The last time I updated was in February last year.

I am hoping that I do not need to update the engine on our side since we have a lot of unit tests that I would have to go through. Unless there have been some significant improvements that you think I should include.

[Julio Jerez]

Got the repro test, very simple thanks.

I pasted in the rigit body demo under if def. Since I get compiler error in float 32.

Did a quick test and God the asset right away.
It the degree of nom pdf is higher than I expect, that could be a sign of some bug, rather than numerical error.
I will have to debug form the top to make sure everything is as expected before doing anything

Btw the mass ration is 15000:1
At least that's more likely.

Anyway I will debug it.

[Esharc]

I am glad you got it.

Thank you for your efforts

[Julio Jerez]

The good news is that, it seems to be a float point rounding error.
I did not make any changes, even when I know that the kinematic loop can case these problems.
but for Newton 4.xx this is taken into account at the spence of slackness by applying relaxation to the main diagonal.

what I see is that the large matrix is in fact PSD, and can be factorized.
but then what happens is that in newton 4.0f the convex quadratic solver applies a matrix partition as a second optimization the reduces the matrix by removing the unbounded rows, And solving the with a direct method while the bound round uses an iterative method.
the problem is that the method is numerically unstable and can make the matrix lose its positive definitive.
It is not a big problem, and in fact is not even an error, since the solver for that matrix is not direct and can handle non PSD matrices.

I will check it again tonight, then I will make the change by using the Cylinder joint, this is very important for a robust behavior.

here is the bad news, on this

The last time I updated was in February last year.
I am hoping that I do not need to update the engine on our side since we have a lot of unit tests that I would have to go through. Unless there have been some significant improvements that you think I should include.

that's more than a year, and the fix for this is on the joint solver, the change do not alter the Algebra, but they are enough that will be very hard to merge so far back.

my suggestion if that you sync and try to merge, while I get the fix ready.
sync should not be too hard, maybe a few interfaces or function.
in some cases, there had been bad bug fixes as well.

[Julio Jerez]

ahh, after going the secund time.
I realize that, the problem is that the tolerance for double is just too large.
but adding the changes, the problem goes away.

Code: Select all

         #ifdef D_NEWTON_USE_DOUBLE
         if (diag < T(1.0e-12f))
         #else
         if (diag < T(1.0e-6f))
         #endif


you can just make that change on your side if you do now want to update to the newer version.

you still have the problem of the over determined loop with hinges.
with the change the solver can converge to a solution, but it is doing at the expence of penalties,

later tonight I will just add that change, plus another tricks. and everything should be just fine.

I still recommend that you update to the latest commit, at least you will get bug fixes and possible easy fixes in case of more complex bugs, but for now, this was just a numerical issue.

the 1e-6 error, is because this is from the very early days of newton, I never thought anyone would come up with an object weighing 1500 tons against a link that is only 100 kilos, back them the condition number was set to about three decimal digits for 32 bit floats.

That's some serious lost of dynamics range that can definitely fail using 32 bit floats and it is still hard with doubles. but double can handle nicely, so it is reasonable to make a different tolerance for doubles.

[Julio Jerez]

one more comment,
I was going to add the cylinder joint, but I notice that for some reason the chain does not move.

so I look in google about Drag Line, and it seems it uses a different mechanism for walking than a four way link https://en.wikipedia.org/wiki/Dragline_excavator

it seems it uses a sliding contact, unless you are trying to model a newer one.

the other point is that, the mass of the main body is 1,500 tons but the cams are only 100 kilos and the foot 1000 kilos.

It looks to me like the Cams and the foot masses are very, very small. Imean 100 kilos, is about the weight of a heavy person.

if you make those cams a heavier, the entire thing would behave better numerically and also closer to reality. unless you made like that to expose the bug.

anyway, if you decide to go with Cylinder Joint, I was mistaken when I put two cylinders, you only need to put one to break the ill condition matrices formation.
but since the diagram is so different, I did not put it because I assume you already knows a lot more about that heavy equipment machines than I do.

[Esharc]

Thank you for the explanation of the issue Julio.

I had not noticed that the chain did not move in the demo that I sent you since it asserted before I could see it. It does move on our side though.

Thank you for the suggestion, I will spend some time this week and weekend to sync again with your work and make the necessary fixes.

Yes the weights were just there to start with, most of the time we do not get exact weights from the manufacturers and we have to guess what they are and then tweak them till we get a solution that works for us.

I will change one of the joints to a cylinder to make sure to avoid the ill conditioned matrix