Talk about inverse dynamics solver

[JoeJ]

if you check it out, tell me what you think.

Did the update to the softer version, but still looks buggy. Feet sink into floor, it jitters, then at some time it flies upwards.
Looks like collisions are not detected a while, then suddenly they do a bit. Pretty weird.

[Julio Jerez]

Hmm... the contact problem i had was not about ragdoll being too stiff. Although visually the stiffness did not look right, the balancing worked regardless. (After switching to the new exact joints, iirc)
My problem form the contact moving to the foots middle only gave less deceleration than predicted, which is another problem and not what you currently see i guess.

not that's not a problem. the way to detrmine if the max displacement of a effector is by analizing the contacts point.
after a the inverse solve is called, to get all the forces, and all the contact.
them for is just a matter if iterating over each contact and for the simple fact that a static friction is given by the coefficient of friction time the normal force.
now you have the desires acceleration for a given motion. and you have the normal force at that contact. so that give you the max acceleration the contact can withstand before loses static friction.
so you can clip the acceleration and determine the max allow. and try the solve again.
like I said before is an iteration proccess that does a few time until in determine the maximun movement of the contacts, it is just that for testing I set the sideway to a very small value until I get there. But I found that as long as the contact keep bouncing it will no work because contact do not get settly for long, teh keep moving sideway each time the bounce.

Tried the demo but looks like you started to try crazy stuff to test things and now it's broken. (No collisions with legs, even external force making it fly upwards it seems.)
So i can't see what your problem actually is.

is no broken, is only support to work when the com is close to the suppose feature, (the yellow line)

once that state is broken a state machine will move the controller to a different state.
the state is broken because the upper body keep rotation around the line that connect the tow sockets. that's a good behavior. all that mean is that there have to be a joint that control the upper body attitude.

the thing you are correct about is that it explodes too easily, we can't have a system that is so fragile. Before moving on I will fix that. so far, I only added warning when that is about to happens, but no action is taken. but that's too brittle need to be addressed first.

[Julio Jerez]

Did the update to the softer version, but still looks buggy. Feet sink into floor, it jitters, then at some time it flies upwards.
Looks like collisions are not detected a while, then suddenly they do a bit. Pretty weird.

yeah, you are seeing how the sausages are been made in real time. there will be bug that we have to fix alone the way.

I fixed that bug, I was using the desired target position of the effectors to generate the support polygon, so you saw the support polygon been draw at eth desires place not the actual place. that a huge bug, but I fixed.

if you can, try again please.

now it will spin until is hit the limit and the blows up. I will see why is blow up and fix that first and foremost.
them add the actuator that maintain the upper body attitude.
and finally, should that happens, and it will for example if it the body get an impact,
the controller should switch to a new state when is move one leg to get a new balance.

[JoeJ]

Maybe you're going too fast by working an walk and balance at the same time already now.

You can (and probably should) work on balance first, with all feet on the floor. Not trying to make step or have some animation as well. Probably that's what you currently do, but i'm not sure, so just saying.

To have some goal, i tried to move the COM to some target position. Within the support polygon ofc.
And my first success was like this:
Visualize my calculated ZMP.
Visualize the COP by making a weighted average of contacts and their force magnitudes.
Latter was noisy, but good enough to confirm i calculate ZMP correctly.
Then, if i want to move COM to the left, i tried to move the COP to the right just a tiny bit. I did so by figuring out the math to control COP, basically using ankle motors.
This let me move the COM only very slowly, but it worked to gain some first balance control.

That just said for some inspiration - not sure how it relates to your method of tackling the problem...

so that give you the max acceleration the contact can withstand before loses static friction.

Haha, that's one thing i never figured out of how to do.
Would be important to move on ice.
But under usual conditions, you can not loose static friction with balancing. You would loose balance long before the foot slides. Be gentle

[Julio Jerez]

Maybe you're going too fast by working an walk and balance at the same time already now.

You can (and probably should) work on balance first, with all feet on the floor. Not trying to make step or have some animation as well. Probably that's what you currently do, but i'm not sure, so just saying.

but I am, this is just trying to balance the upper body in the two side legs for as long as it can.
it is actually doing that, but it needs some more control on the upper body.

Haha, that's one thing i never figured out of how to do.
Would be important to move on ice.
But under usual conditions, you can't loose static friction with balancing. You would loose balance long before the foot slides. Be gentle

yeah, that's courtesy of the immediate solver. Before that, the only way to get that info was by using previous contacts.
And yes, you are correct, under normal conditions, a contact should take a fairly large sideway displacement. So we can use that to calibrate the model using small displacement that we know are small enough that will not cause the contact to slide.

In the final code, when the test that check if some desire displacement will cause a contact to slide, that's a signal that can be used to trigger the state machine to move another close limb, a recovery state.

For now, we just need to make the static balance stable. any Dog can do that when trained. the controller should be capable of doing that in all cases, but what we found by this is that there are missing actuators.

[JoeJ]

I see you removed two legs. Looks much better, no more jitter.
Funny: The torso rotates slowly down. But it crashes after two seconds, so i picked it up a bit and sat it down again. It behaved the same again, slowly rotating, then Newton did put it to sleep and freezed it like an upright egg
Seems unintended sleep state is really a hard problem for physics engines

But i can see it working on preventing to tip over

[Julio Jerez]

Ahh excellent, Thanks for checking it out.
also, you check the validity of what I am saying Joe.

if you place a break at this ndJointHinge.cpp line 231

Code: Select all

ndInt8 ndJointHinge::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)

and you running. you will see that is touched that line well before is goes wacky.
once that happens them the configuration changes. so some action should be taken well before it riches that point.

I good controller should deal wit that.
Remember you were the one who told me that limits are really hit, so this test so demonstrates that.
there is lot of work to do, before making the controller to prevent that. first thing first, we need to handle that limit problem.

[Julio Jerez]

Funny: The torso rotates slowly down. But it crashes after two seconds, so i picked it up a bit and sat it down again. It behaved the same again, slowly rotating, then Newton did put it to sleep and freezed it like an upright egg
Seems unintended sleep state is really a hard problem for physics engines

yeah, when the tow point reaches the aquarium point, is doing that by asserting torque on the hip ball joints. but once is reach that point, the upper body in in equilibrium on a straight line.
that is an indifferent equilibrium point. There could be an infinite number of rotation that satisfy the same condition, and since the upper body has much mass, it angular momentum keeps spinning it and the controller making the adjustment.
I hope this makes it more clear, that it needs some more muscle that add dome drag to damp that angular momentum. or even better a muscle joint that is active and allows for the upper body to be controlled so a desire orientation.

That trick you do, lifting a lithe after it reach the com, what is doing is zeroing it out the upper body momentum, so the body no longer keep rotation and go to sleep. every if is awaken ever time step.
to me that a very encouraging sign. all we need to do is add that extra active joint to control that.

The sleep state is actually a good thing because it means the physic is finding solution each tick.
The model is actually awaked in every update, so it is very good that we see in blue, because it means it bringing it to equilibrium in one tick almost every time.

[JoeJ]

I hope this makes it more clear, that it needs some more muscle that add dome drag to damp that angular momentum. or even better a muscle joint that is active and allows for the upper body to be controlled so a desire orientation.

I'm not sure what you mean, but i guess you want to stop the 'slow rotation' of the torso? How should additional muscles do that?
Maybe you just missed an important difference of biped vs. quad for a moment. Bipeds have a large foot, and the can move COP within this area, so the ankles alone can do balancing while keeping torso just straight.
Dog feet are too small, their ankle can't touch the ground, so they need 3 legs in contact to move COP in all directions to have stable balance.
So i see nothing wrong with the rotating torso on just two dog legs?

so it is very good that we see in blue, because it means it bringing it to equilibrium in one tick almost every time.

Ha ok. I see

[Julio Jerez]

Haha, that's one thing i never figured out of how to do.
Would be important to move on ice.
But under usual conditions, you can not loose static friction with balancing. You would loose balance long before the foot slides. Be gentle [/quote]

one more thing Joe, on the movement of the joint, it is actually even simpler. in fact so simple that we can add that right away.
but first let us clarify what I mean by moving the effector.
when the effector moves, it does not necessarily do so, it is a constrained motion.
we are not moving the effector, we are calculation an acceleration that will generate a relative motion. this could be the effector moves or the other body moves or a combinations them moves.
if an effector is unconstrained, a foot on the air) the final result will be a nonzero acceleration on the body. so the effector will move.
bot let us say this effector is controlling a foot that is planted on a floor.
the criteria for if static equilibrium is that the acceleration tangent to the contact normal is zero.
so in this case that effector should not move, but the other bodies should.

so if we iterate over all contacts and we find that any of then has a no zero acceleration tangent to the normal, that's the sign the foot will slide.
that's a solution, however we can do a lot better.

check out the free force body diagram.

Untitled.png (8.28 KiB) Viewed 3443 times

[quote="JoeJ"]

the red line is the body net acceleration after the solver step
that the state of the body controlled by the effector aft ethe inverse solve is called.

it does no has to be zero.
let us say that somehow some contacts are broken but some are not.
that make a hard problem to determine if the displacement is a good one of not, for example say the body is about to pivot about some contacts.

the problem reduces to projection the red acceleration over the desired displacement and if the resulting value is not zero, it means the foot will slide. but the acceleration can still have a not zero value along other directions.

so how much would we clip the displacement can be as simple at the halving the desire displacement and recalculation the acceleration again.
I guess that but doing tow or tree time should be enough, and if it does not find a good displacement them it mean it will lose support.

so you start by the max displacement, maybe some reasonable caps. calculate acceleration and check is the support effector will slide, if the do half the displacement and try again until it either find a displacement.

that result is that, if it fails the first time it means it will lose equilibrium, but the net try will just try to do some attend to get equilibrium. this is good because even if it does not get equilibrium on the frame it try it best, so in a next frame it might get it.
anyway they are many way of doing this.

[Julio Jerez]

Afte trying few thing whiout susscess, I think you are right the model is too complex to develop the algorithm.
Even when reducing it to two legs. In fact, now I think that even with one articulated leg will be problematic.
Somewhere there is a positive feed back that I haven't figure out, and that happened in the pass too.

So over the weekend, I will make a much humble model.
A pogo on a stick.

Basically an big box with one limb and an effector and try to balanced that.
This removes all of the code for finding suport point an all that junk.

I going with the asumnsion that a pogo model has enought symmetry that I will know what the solution should be and see where i am going wrong.
Them if that is successfully we can make with one full articulated leg, and keep adding complexity until all the unknown are resolved.

[Julio Jerez]

One of the things I have not done is in fact a huge oversights.
This is that I have not added tge criteria to determine if the system in in fact controllable.

Basically this is a way to say if a system with a fix point solution can be stable by adding a perturbation.

A fix point in control any point of interest in the equation.
This could be any point, but in general the pools and zero are selected as fix points
For example, in a inverted pendulum there are two interesting points.
When the ball is at its lowest and when the ball is at it highest.

It is very hard to get deferential equation for anything more complex than a simple pendulum ir double pendulum, but the fix point are a important comspset in controll theory because you can say the a small perturbation can be seen as a linear funtion of that state.

All tha mombo jumbo is just Flux, tge importan part is that when you get the mass matrix of the system at a fix point, a way to determine if the system can become stable is if the higher values of tge mass matrix are all negative.

We know for a fact that if for example a pendulum is at it lowest the hiegen value are all negative, this mean a small perturbation will make the system com back to a stable state as long as the mass matrix of the perturbation state also has negative hagen values.

If for example tge pendulum was at it highest it will have at least one positive hidden value, so if we add a perturbation that make those positive values negative, the the system is controllable.

But if the hinge values are so large that not about of perturbation will make negative, them it will just fall

I will make the inverted pendulum pogo ti test that out, which will reduce to a funtion that we will call the and will report the heigen value of that state.

There is code fir that in newton but I have not applied to the Skeleton solver. I used in the pass fir contact discrimination, but fir 4.00 I decided to go with a geometrical approach.
But all the code is there.

Any way, we can't be so cavalier and ignore basic controll theory, because some system are just not controllable and I suspect there is some of that going on.

[JoeJ]

We know for a fact that if for example a pendulum is at it lowest the hiegen value are all negative

Oh my... so many tools are missing from my bag, and i do not even know about their existence.
I guess you meant eigen values. That's something i still have no clue about. I'll die stupid.

... but i'm a creative bastard! Could not find an analytical solution to project twist limits to the solution circle, and already decided to give up and use a shitty iterative approach to search for it. But that was not robust in all cases. Now, after visualizing the iterative results over the whole cirlce in a graph, i noticed some symmetry. The desired correction is the same as the error i get from PI-angle.
So i have my analytical solution. I don't know how or why it works, but it does.

[Julio Jerez]

Oh, but at the end of the day all the engine value talk become just a pedant jargon.
but the principle are important.

basically is comes down to the fact that a kinematically constrained arrangement of rigid bodies is a linear system on instantaneous velocity of each body.

after all the formulation the joint forces can is give by the equation

1) A * f = a

A is the mass matric
f is the constraint forces vector,
a is the joint acceleration vector.

is we think this equation as just an instance of general differential equation of the system.
furthermore, if we consider that this was the solution of the system after a small perturbation system of the equilibrium state.

Then there could be a matrix B * u that if added to the above equation most make the system go back to the fix point (fix point being the equilibrium state)
in general for a open loop system A is positive definitive, but for a close loop is not. so we we look for is the values of B that make A psd.

this is determent by decomposing A into it eigne values, that is
A = J E Jt

E is the diagonal eigen values of A
where J are the eigen vectors,
if is we replace it in equation 1
we get

J E Jt f = a

and by factoring you get

E * Jt * f = Jt * a

Je is simply an orthonormal set of vectors (basically a rotation matrix in higher dimensions)
what this mean is that

Jt * a

is just the rotation of the joint acceleration to some high dimensional space make teh f value independent of each other.
so when can call that space

f' = Jt * f
a' = jt * a

and what we get is a decouple set of equations

E * f' = a'

all in one dimension
and now you see that E is just a scalar of the acceleration, so if in that space if a value of E is negative it will simply make the acceleration change sign, so the task is find B that make that row positive.
this is just academic but can be used as a metric for how to adjust the system properties.
for example how long a leg can be, how heavy the to weight can be and so on.

use you get the idea this thing are determine mostly by intuition and trial and error. but we still have a system that is incomplete and may have plenty of bugs that nee to be resolved.

anyway I just started the simpler inv pendulum to try that out and see if we can make and.
them it will print how the eigen value change as the control of the effector are applied.
and what can be make the effectors be more effective.

in the end eigen values is prohibit tool, to use, I try in newton 1.xx to reduce contacts, and is was just too slow, but it is a valuable tool for sanity or health of making system.



you can check, you tube videos about Lagrangian dynamics and invert pendulum on a card demos, it seems that each graduate student who learn Lagrangian dynamics feel the urge to make a video using it.
application liek Mathematics and Mat Lab even comes with those example already made. I have Mathematic.
the problem is that Lagragian dynamics one you pass the simple textbook examples, is becomes untratable because is very easy that that a system lose DOF and also is no that eassy to figure out the generalize coordinate.

[Julio Jerez]

I already set up the simplest IP I can thon up, and I already see the first problem.

It has perfect symmetry, so it all starts very nice, but the moment I apply any small perturbation by touching it with the mouse, is simple falls, and behave weird,

that is because the ik joint produces some acceleration but is not aware of any velocity that was already on the system already, so the joint keep moving, this is normal.
what is no normal is that the stick seems too strong, it should stop went is hit the ground because is not massive enough, but is handling the much heavier box as if it was Cotom,
that seems like huge problem that need to be investigated.