Talk about inverse dynamics solver

[Julio Jerez]

ah it seems on lesson is that a 10 to one mass ration for the leg is actually too high.

the leg with a mass that is 1/10 the mass of the body is too energetic, so I can see that is the joint is motored, them even is too skinny is apply too strong impulse to the body each time a change of momentum happens,
that seem counterproductive for biped, if we compare to human's which seem that the two legs are about 1/3 of the body weight.
but it is not for quadruped, dog, cow, lions etc have skinning legs.
and if we look at insects, the legs are ultra skinny, about 1 is one hundred the body weight.
I guess is because insect move their limb really fast, having a big mass would be hard to balance.
Mammal moves slower so they can have heavier limbs than insects, and humans and elephants are the slowest, they can have strong heavy limbs.

ideally you do not what a too strong reaction from the body from moving the legs.
and I saw some of the in the quadruped.

I will try 1/20 and see.

edit: much better and predictable. moving alone.

[Julio Jerez]

more discovery,
if there is only one spherical limb, them it can't have an effector since the workspace for the effector will be the surface of the sphere at the of radios from the ball socket to the effector pivot, so that only two dof.
there is a lot to learned when doing this stuff in practice.

but anyway I remove the leg and leave the effector and now the Pogo bounces on the support.
It does not fall to the side, but that's because it has perfect symmetry.
now the trick is to place the pivot to the side, or to add some side impulse and see if we can get it to keep the balance.
also, if it keeps its balance, how much of a side force it can tolerate before it falls.

[JoeJ]

While searching images about 'Pogo stick' i found out there is an official political beer party in my country. Their program is 7 days weekend, to get rid off the 'folk drug of working'
Don't laugh. They seem actually better than the others of our politicians.

After downloading your demo i saw pogo stick is actually a one legged hopper
I like that. Would be a nice kind of enemy. Once moving, it can't stand still. It always has to keep hopping. That's fun. And to beat them, you try to tip them over.

Reminder: https://www.youtube.com/watch?v=dmIjMEbs0Ww
Actually i was thinking about this when you mentioned a guy landing on stretched feet.
Like with the hopper, i would handle this with a higher level controller shortening the leg to damp the impact.
I wouldn't had expected Newton to handle this for me back then

[Julio Jerez]

Ha yes that video is really cool.
The controller should do that for walking. But should not shorten the leg, it should only count on the friction on the leg to do side motion. If you look a pendulum on a card videos in you tube, you will see that this is a classic text book problem in a dynamics controller class course.

The jump is a secund part of the problem.
Say for example, he knows how to keep his balance by applying small sideways tug up to certain limit.

The limit is determined but the max friction.
Whe thus happen what he does is that he move the secund leg toward the direction of that he is going to fall.

For that, it can predict the movement of the secund foot with the kinematic equation of an imaginary Ip.

At that point we will have a walker with two sticks legs.

From there we elaborate by adding the two part of the legs:, thig and calf. And we keep elaboration.

But the most important problem now is to learn how to move the effector horizontally to prevent it from falling after small perturbation.
Remember, a sideway motion is limited by the friction on the feet, that why the very stiff spring.
Last year, I was trying to model that with a soft contact and that was a huge failure. The idea of the stiff spring with strong dampening is to model what a knee and uncle with do to soften the impact each time a foot hit the floor.

[Julio Jerez]

Joe I looked at those other two videos that you have there.
I remember seeing them few years ago.
I under stimated how impresive that achievement was.

I remember you mentioned you were using inverted pendulum.
Can you say something about how you implemented.
Maybe that could be a start point.

By objective is to get to a point where this is formulated by a kind of a cooking recipe, and not by black magic art.

I assume you are calculation torque.

The way I see it is that it is possible to go from the torque an forves to the displacement.
I am not making this up.
This is a fundamental principle of dynamic called the D'Lambert which is by far the most common method for calculation joint forces in mechanicl engineering.

This principle is a special case of Lagrange, it the case where the acceration is zero. So I am bending it a little to support acceleration. But so far each time the model is accelerate seem to fail bad but whe the robot is grounded works great.

Maybe we can recreate you model in the new sdk.
And from there we can see how far off the inver method.

If you look at the I durian robot, there are two demos.
One with the effector as part of the robot. And the secund with the effector as a tool to calculate the joints accelration.
The I comparate the resulting acceration on each body.
And they are the same with in margin of error.

Of course the big difference is tha for the industrial robot the target acceleration is zero. While for the balancing pendulum making that goal make fail miserably. There has to be some dynamic equilibrium, this is eluding me.

[JoeJ]

Ah *!
I just wrote my IP approach in detail in lengthy post, but was already logged out when posting so it's all gone. >:(

Will write again, but not now...

[Julio Jerez]

oh that will be helpful.

I now added the part of the code that so far has worked. it does not do anything.
it just got to the point when is has the contact, and the com target contacts.
since the model is in equilibrium those point are identical (with in margin of error)

the next step is to offset the pivot and so that those tow point are different.
that is if the effect was a leg it socket ball will be to the side of the hip. so I will try to offset just a little.

the next step is to see the translation that the projection of the com has to translate to met the contacts. is does that by asserting torque on he ball.
as before the limit is how much the start friction at the contact will allow to apply before start to slide.
This seems like a trivial reasonable action but so far is had failed.
so I am hopping this simple model allow me to find out what I am missing.

edit:
if you check it out at line 67 I added this

// try offsetting the effector.
sphMatrix.m_posit.m_z -= 0.01f;

is enough for the pogo to land and start to fall slowly , so now let use see if the action is enough to counter the effect of gravity try to bring him down.

now you can see that is a tug of war, the effect generates a torque, pivot on the contact friction while gravity generates a torque in the opposite direction.

if the gravity torque is biger it will fall, if the effector torque is bigger will will stop the effect of gravity.

howevert, that is no enough because in down that it seem to add too much momentum to the model and it over short, that when the dynamics takes effect.
that all that all the eigen value jargon means,
basically if the gravity torque is too large, them there amount of torque need it to stop the motion will be so high that the model will over should, because it will no stop when it reach equilibrium.

the will happen for many reason, a external perturbation, an internal action to move. or the geometric configuration like if the leg is too far off the com,

right now this last one is the one I am try to control, but the method should be good for all cases.
anyway we keep going.

[JoeJ]

IP.png (11.11 KiB) Viewed 3193 times

...decided to draw image instead much text.
For the hopper i did not use IP model at all. It's support is only a point, so no stable balance is possible.
I used this only for the ragdoll which has a support area, so it can balance.
Beside the ragdoll, i also made a 2 bodies model like in the image, with a spherical (or hinge) powered joint. IK isn't needed for just two bodies, but it's a good test case to figure out a balance controller.

If the joint isn't powered, the COP (center of pressure) would be just below the COM.
And as long as the ZMP remains within the support polygon (defined by the wide base body), the ZMP and COP are the same.

For the IP we can find equations of motion which describe the trajectory of the falling upper body.
I also found equations of how we can position the COP at will by using a motor to generate ankle torque. I used the torque equation to calculate a pose for the next frame, which then caused Newton to generate the torque i wanted.

At this point we can make a naive controller by nudging the COP slightly left and right from the ZMP.
As long as the COP remains within support, the foot (base body) stays flat on the floor and does not tilt or move.

This way we gain control over the upper body, while the base remains static.
And we defined our constraints of keeping COP within support all the time.

Summing up: The COP is the 'cart' referring to the classic text book model. (I've drawn small carts to illustrate how placing the cart left or right makes the upper body 'fall' right or left.)
But we can only move our cart in the constrained support area, not beyond. If we try, foot tilts, and we loose balance rather quickly.

To do better than the naive controller, we do not just nudge the COP slightly from the ZMP, we move it directly to the edges of the support poly. That's the fastest motion we can generate within our constraint. (Which in practice is still slow for a biped.)
The calculus exercise now is to calculate when we have to switch the COP to the other side, so we switch from acceleration to deceleration phase in time to ensure we can counteract the falling upper body, so it stops at the target position and does not fall over.

That's all i have used IP model for. The IK stuff then is only needed to make the ragdoll match the simple model.

[Julio Jerez]

ah thanks Joe.

I now try to apply some control to the pogo model, and I found few the very method I try for the quadruped as much as possible.
This is not complete, but my objective is to find where I was going so wrong.

I did find few thongs which to me is a good sing, I will be very disappointed of this was worked using the same method because it means something else was wrong and it was on the elements removed from the more complex model.

this was the major finding.
The support is off by 5 cm form the center of the Box on top, so it will gradually start to fall, and he job of the control was to see if is can balance itself by moving to the opposite direction of the falling velocity.

to my surprise, it fell as if not effect was taking place, but stepping in debug mode. I found that the spring the PD action was no string enough.

so I exaggerated the effector motion, and at 100 time is does balance it for a while.
it does spin and moves down, by that's because I am only controlling the horizontal displacement, thsi si so that I can model the quadruped, but now I can add more thing to the controller until wil gat to balance completer and stable.

you can try this by syncing and looking at line 255 file ..-4.00\applications\ndSandbox\demos\ndInvertedPendulum.cpp

Code: Select all

      float applifyEffect = 100.0f;
      matrix1.m_posit.m_x -= step.m_x * applifyEffect;
      matrix1.m_posit.m_z -= step.m_z * applifyEffect;
      ndMatrix targetMatrix(matrix0 * matrix1.Inverse());


after I add the rest if the controller part, like keeping the orientation and relative distance to the floor.
them the amplification factor is an indication that a PD, is not probably the correct controller.
PD stands for proportional derivative.
It seems that what is need it is a PID which is proportional integral derivative
a pd apply a correction of the form

Code: Select all

a = - ks * (x - x0) - kc * v

where ks and kc are the spring constant
the controller easy to make and calibrate because when using implicit integration, they are unconditionally stable because they are super critically damped at worse critically damped,
but they the problem that they require an error for make a correction, if x - x0 is too small, them the effect is also too small, and that the big problem we are dealing with very small errors.

a PID uses a memory to store the error. the equation is

Code: Select all

e = e + (x - x0)
a = -ks (x -x0) - ks1 * e - kc * v


we can make ks and k1 equal or not, for simplify I will make equal

e = e + (x - x0)
a = -ks (e + x -x0) - kc * v

which is almost identical to the PD, but now you can see that e is an error accumulation so every time the error is not zero, is add to the memory, so when even when the error is a try zero, e is not so it will make a correction.

these controllers are much harder to calibrate, because as you can see, when the x - x0 become zero
is e is not zero the controller over should, and is the kc is not strong enough them PID can be under damped and explode, but I guess this is part of the control Job fond that Ks and Ks1 that make ethe error effective.

but any wat if you try you will see what I mean.

[JoeJ]

Oh no, i don't see what you mean.
I remember to have learned about PD, PID, or spring dampers when i worked on my own physics. But that's almost 30 years ago, and now i have forgotten it all.

It looks like you try to control it by rolling the lower body on the floor? So it resembles a true cart?
That's interesting. With my COP example, you can basically teleport the cart, moving it instantly to a target within the base body in one frame. So that's a very different control problem.

The upper body starting to spin probably isn't intended?
I remember some detail about my hopper: I used one joint which could swing the lower body within a cone, fixing relative rotation, but also could do a slider to shorten the leg.
Interestingly this worked, but only if the joint was attached to the com of the upper body. Otherwise it started to drift like from external force.

[Julio Jerez]

Few thing are wrong.
The fact that is gaining so much momentum does not seem right.
I would expect that when the small ball roll to one side that the top box would roll in the other direction by a much minuscule amount since it is about 100 time more massive.

But is am seeing result that do not seen right.
There is mush work to research. It seem there is a fundamental flaw some where.

[JoeJ]

Yeah, i thought the same. It looks like the top should fall to the floor pretty quickly, but doesn't. After some seconds the bodies stop, but the top one keeps levitating in air although the bottom ball is way off of the com, so it should fall.

[Julio Jerez]

no, no Joe. on this

It looks like the top should fall to the floor pretty quickly, but doesn't. After some seconds the bodies stop, but the top one keeps levitating in air although the bottom ball is way off of the com, so it should fall.

that part is what is right and expected. what is wrong is that the effector keeps trying to apply control after is find the equilibrium. and that where I believe I either have a bug or something is missing.

it is important to get a clear definition of what is that we are trying to get.
Basically we are trying to make a system that is on unstable equilibrium to become stable.
There are three type of equilibrium:

Untitled.png (47.38 KiB) Viewed 3149 times

stable is when the system reaches a point where any perturbation makes gain potential energy so it restore back to is position of minimal energy.
the secund, indifferent is when a perturbation does not add energy.
these two are un-interesting, because cause is very little that can be done with a system like that.

the third one is the interesting one, it is when the is a point where any deviation from that point makes the system lose potential energy so it will just keep falling.
this of a ball on the top of a mountain, a gust of wind will make go downhill. now think of that ball having a small rocket that can pointed in the direction opposite to the wind direction, is the rocket can match the wind force the ball will state in equilibrium, is it can't it will lose it for tow reasons.
1-the wind is too strong, and the rocket can't match it.
2-the wind is no strong, be the rocket overreact and push it to the oversize.

Unstable equilibrium is the cores of everything in control theory and even nature. for a system to keep that equilibrium is mean that is need a source of energy to generate internal force that can counter act the of potential energy, meaning is most consume energy

in control theory as I expressed before the equation reduce to this, in some space the variable of the system will be determine by the linearization of the equation at that state,

so teh equation will be

Y = A x

but on A there will be a positive coefficient

so to make a system like that stable, engineer add a control variable

y = A x + B u

when the coefficient of B added to A makes all values negative and the system becomes stable.

u, is the input variable. and B is determined by the control that we are trying to model with a PD, and now with a PID.

the problem is that for anything more complex that a pendulum in 2d or a ball running downhill the values of A are very hard to find analytically, however engineers has been downing it for hundreds by trial and errors, that what control theory really is. Now people are trying to doing with neural
networks. the point is, that it very intuitive to get a measure of the system and apply a counter action to correct it, what is hard is to gage how strong or week that action has to be.

but anyway, the test do shows sign that is down the work at least for few seconds, but it is obvious that there is something either wrong and missing that nee to be determined.

finally, imaging a person standing in one leg. if someone give a lithe push, depend on how strong the push is, any person is capable to keeps its balance but tilting its body a little,

we can see that the upper box does lilt, to match the com, but what happen is that in order to do that it adding angular momentum, but once that is reach it does no know how to stop and keep adding more and more, but in doing that each perturbation becomes larger and larger until it can't keep it anymore.

what I expect, is that is tilt and after a while stops correcting. and will be floating

[JoeJ]

the problem is that for anything more complex that a pendulum in 2d or a ball running downhill the values of A are very hard to find analytically, however engineers has been downing it for hundreds by trial and errors, that what control theory really is.

I have heard PID is the swiss army knife of the engineer, and he spends all time tweaking its values
But in the end it's always a kind of error feedback, reacting to the past?
What i do is predicting the systems future, and reacting to that instead.
However - just keep going. Even if it turns out not good enough, the path of failures is unavoidable to find enlightenment at its end.

we can see that the upper box does lilt, to match the com, but what happen is that in order to do that it adding angular momentum

You mean your system creates a flywheel effect to keep balanced?
And it does so automatically? That's pretty smart then.

But that's not what looked wrong to me. After two seconds, the rotation of the upper body stopped, no more motion at all, an the system came to rest at a state like so:

Code: Select all

XXXXX
XXXXX
XXXXX
    \
      \
        \
          O


It should fall to the left, but it didn't.
That;s clearly wrong, similar to how i saw it flying upwards some days before.
You might know the reason, but just telling.

[Julio Jerez]

Not it would not fail Joe.
Whe the body is in the state you show in the sketch.
There is a dynamic equilibrium. The body try to fall because the pivot is offset, it reduce that torque by increasing the relative momentum between the two bodies, it does that by increasing the relative velocity between them but it dies no know when to stop.

That's part of the problem.
In the beginning g they are offset and that momentum is needed to go to the equilibrium. But once the top body has that momentum, the lower has the opposite, so in order the get more torque it can only increase the relative speed and they keep separating. This has to be a bug.