Unsteady simulation problem

[JoeJ]

COP out of bounds.jpg (85.28 KiB) Viewed 7000 times

The image shows inverted pendulum swinging left and right at maximum speed.
The problem is, the center of pressure (COP) must stay inside the support polygon,
otherwise the pendulum becomes unbalanced and tipps over.

The support polygon is the small quad on the floor marked in green
(the real body is larger only to study the problem).

The predicted COP is the yellow dot inside the support poly
(plot over time at top left of image in green).

The simulatad COP is the white dot outside, measured by averaging contacts weighted by contact forces. (plot in blue)

The red plot is angular acceleration fed from the controller to the joint.
(measured acceleartion from sim is too noisy to make sense)

Usually the predicted COP follows the simulated nicely (i proof my math is right this way).
The problem is, at the time the pendulum stops and reverses direction, the simulated COP jumps out of bounds for 1-2 timesteps and that's enough to loose balance.
I only observe this problem when the top body has nonuniform inertia, otherwise small thresholds are enough.

I tried some things: Exact solver, hard joints, more iterations, 120 Hz, using a hinge joint to avoid gramm schmidt, but nothing helps.


Is this expected when a joint reverses its motor direction?
Do you have any Idea to handle this?
(Maybe there is even a physics law in action i don't know yet, like gyroscopic stuff...)

Actually i see only one possibility:
Shrink the support polygon so the jumping COP stays inside the real contact area.
But that means much smaller max accelartion - i'd need to make pretty large feet to compensate that.

[Julio Jerez]

I am actually very interested on this for making a simple walking demo on and uneven terrain.
something like a T Red.
what does this means?

The problem is, at the time the pendulum stops and reverses direction, the simulated COP jumps out of bounds for 1-2 timesteps and that's enough to loose balance.

I never tried to reverse a motor direction. I suppose that is not different than setting the motor to
zero. basically it comes down to a change on velocity which is just an acceleration.

My guess is that the soft joints will not react fast enough to sharp acceleration changes.
neither would the newer exact solver since it was using iteration for dealt with unilateral joints.
I just checked in the version that solve hard unilateral joints, can you try again using the Skeleton solver?
Can we reproduce this in one of the standard joints demos?

[Julio Jerez]

did you look at the paper Joe?
http://graphics.csie.ncku.edu.tw/IPM/ba ... 629_01.pdf

I believe that once we get the hard joint going, the entire system can be implemented trivially without using the PID mombo jumbo.

[JoeJ]

reversing motor direction was a bad term. What reverses is velocity at a time while the motor sets almost constant torque, like a ball changing velocity at peak hight after throwing upwards under constant acceleration.

Look at the small peaks at the plue time plot.
That's the positoin of the measured pressure point from newton contacts.
This shows that Newton either applies more torque than necessary, or slightly wrong contact forces.
I tried to set joint max friction to the torque that should be correct, but that did not help, so i guess it has to do with contacts.
(Take anything i say with caution - this stuff is still difficult for me and it's hard to be sure my math is right)

It's not a big issue in practice i think.
The controller i use here works for a simple 2-3 body setup, and it seems to work for the ragdoll using hard joints, but with soft joints there will be no thing like a perfect analytical controller - it remains an issue of manual tweaking here and there.

I am actually very interested on this for making a simple walking demo on and uneven terrain.

What helped me the most was this paper https://www.google.at/url?sa=t&rct=j&q=&esrc=s&source=web&cd=2&cad=rja&uact=8&ved=0CCsQFjABahUKEwix8_XK3KHHAhVJiRoKHRpUBVs&url=http%3A%2F%2Frepository.tudelft.nl%2Fassets%2Fuuid%3A9e390296-9ba9-46d5-9b3f-3b7653b2d1ba%2FThesis_Sebastiaan_Kiemel.pdf&ei=fUfKVfHAKcmSapqoldgF&usg=AFQjCNFSJJKJA1c26bbxlR1fi1NqvC-jgA&sig2=HWl6Hh1P6NFa16njKwUWiw&bvm=bv.99804247,d.d2s

The math formulas here are easy enough for me
There is how to calculate the Center of Pressure,
and the Instaneous Capture Point (ICP).
If COP == ICP and both are in the support polygon, the robot is balanced,
So a controller can calculate a simple ankle counter rotation from (COP-ICP) * userGain

All the videos i showed use this strategy - it does not tell you how fast you can go to ensure not loosing balance because overshooting target, but it is surely the best first step.

[JoeJ]

I just checked in the version that solve hard unilateral joints, can you try again using the Skeleton solver?

Can't answer now - for some reason the inverted pendulum with hard joints test does not work as good as yesterday, also if i move back to older newton, so i must have messed something up...

Can we reproduce this in one of the standard joints demos?

When your powered ball socket works, create a two body model like in my picture.
Set target angle from sinewave (nearly the same motion),
Measure contact forces and visualize averaged contact from those.
If the upper body is a sphere the contact should move much smoother than for a nonuniform inertia body.

did you look at the paper Joe?
http://graphics.csie.ncku.edu.tw/IPM/ba ... 629_01.pdf

I believe that once we get the hard joint going, the entire system can be implemented trivially without using the PID mombo jumbo.

I can't even read the first equation in this paper. I always have to reinvent wheels because i do not understand when someone tells me wheels are round... that can be frustrating, believe me
However, reading the text it seems very very similar to what i'm doing.
I seperate the ragdoll to 2 groups of bodies, calculate 2 virtual bodies with matching physical properties for pendulum, and porject the motion back to the real ragdoll with IK solvers.
Never in my life i will try again to do anything with a PID controller - there is always a better way.

[JoeJ]

sine_soft_vs_hard.jpg (93.11 KiB) Viewed 6968 times

I made the test working again, this time setting joint target angle just from sin(time).
The soft joints show an additional turbulence when upper body passes the center of the lower body.

Replacing the top body with a sphere, the center turbulence disappears, but those when changing direction remain the same. (I was wrong thinking it's related directly to inertia)

[Julio Jerez]

where do you see the turbulence? the two pictures look the same to me?

It looks like there are a bunch of people trying the inverted pendulum stuff.
I fund a more intuitive paper.
http://graphics.cs.cmu.edu/projects/run ... /final.pdf

The firs one I posted does not look that instructive and for what I say, it doe no seem that would be stable, reason being that if use tow pendulum one for each foot, that will bring big coordination problems.

I will read that secund one which seem simpler.

[Julio Jerez]

reading over the article the describe the IPC trick like this

An inverted pendulum on a cart (IPC) is a rigid body system
that has two translation joints to move the cart and one
rotational joint for the pendulum (Figure 2a). Because the
rotational joint is unactuated, the inverted pendulum is in-
herently unstable and must be actively balanced by moving
the cart horizontally. This property resembles the balancing
actions of humans. The leaning angle of the character’s body
in dynamic behaviors such as turning, accelerating, or maintaining
balance can be effectively modeled using the IPC.

so basically the trick is to balance a stick, which in inherently unstable but that is quiet ease for
a human.
so essentially the system is a mass resting on a stick which can translate freely and the code nee to generate the velocity the will keep the stick in a unstable equilibrium

[JoeJ]

where do you see the turbulence? the two pictures look the same to me?

In the blue plotted line on the right side, they show x coordinate of averaged contact point over time while swingeng left right.

However, i tried reducing max joint friction again and this time it worked - the peaks of turbulence disappear So the problem can be solved that way and COP can be forced inside support polygon.

reason being that if use tow pendulum one for each foot, that will bring big coordination problems

At the moment i use 4 pendulums, one for each foot, one for combined feet (i blend their results depending on where the weight is), and another for the hip.
I want to do the feet with a single pendulum and blend it's setup instead, but it did not work well first time i tried.

If i can't solve the "fast move weight between both feet" problem soon, i am afraid i need to develop a single '2 parallel pendulm' model.
I thought the hip might play a big role, but i got only a speed up of 5%, so it seems the ankles alone do all the important stuff.

so basically the trick is to balance a stick

Yes, exactly!
The base of the stich can be either:
a cart
the center of pressure inside the feets support polygon
the stance foot, while the target for the swing foot would be the ICP to stop walking.

... it's always the same thing.

[Julio Jerez]

O I see the lithe mark on the blew sine wave is the jitter.
I am still confused, to want you apply the sine wave?
is the sine wave some torque of force applied to the top body?

[JoeJ]

is the sine wave some torque of force applied to the top body?

no, no - that's misleading - torque, angle, velocity - everything will look like a sine here
i'll give some psoudo code:

Code: Select all

joint->targetAngle = sin(time * 0.1) * 0.2;
// joint will move to this angle next timestep, makes body swing left - right

// measure center of pressure, which is the blue plot about
vec cop(0,0,0);
float weight = 0;
for all contacts of footbody
{
    float gcf = fabs (contactForce.Dot(gravityUnitDir));
    cop += contactPosition * gcf ;
   weight += gcf ;
}
cop /= weight;
Plot (cop.x);


Hope this clears it up.



Here's a nice picture showing pendulum on cart and balancing on foot COP is the same thing:

http://www.elysium-labs.com/robotics-corner/learn-robotics/biped-basics/zero-moment-point-zmp/

ZMP == COP, as long as ZMP is inside support polygon.
Naturally COP can't go outside foot body.
If it reaches the edge of the foot, the foot body starts to rotate and tips over - balance lost.
That's the constraint it's all about.

EDIT: made pseudo code more accurate

[Julio Jerez]

Joe I was look for more info on the net and I see some very simple animation of the IPC
this was the simplest I could fine
https://www.youtube.com/watch?v=Py3SMHi3Isw

is that what you are doing at it core?
so basically it all comes down to the simulation of a game of POGO. is old we game we the poor children use to play when I was a kid. It only requires a broom stick. who knew that was going to be so usefull.

this paper seem extremity eassy. I now see how the Pendulum can be associated with the a charted to force to keep the balance. what a clever idea. Tip my hat to however came up wit the Idea
http://www.xbdev.net/misc_demos/demos/i ... /paper.pdf

The thing is that using the physic engine we do no have to write the hard differential equation, the join will handle that naturally provide that the work as expected.
I think now I can make the demo of the T Red of the Giant Mechawarrior
Maybe I find he problems you are finding and we can come up with a solution.

[JoeJ]

is that what you are doing at it core?

Yes, but the cotroller in the video seems bad. It fails to stop the pendulum.

You are right about the comparision of a child balancing a stick.
I remember how cool and easy it was doing this when i was a child myself

Read the chapter "Analysis and control using the Instantaneous Capture Point" page 12 in the paper i have linked. This is a very solid and easy controller that allows to control the cart to reach and hold a target with the pendulum.

The idea is: You have a pendulum that begins falling.
You calculate the ICP (= "Instantaneous Capture Point", not "Inverted cart pendulum")
If you place the cart (= COP) at ICP position, the pendulum stops falling.
If you move cart short to ICP, pendulum falls slower.
If you move the cart beyond the ICP, pendulum starts swinging back to initial position.

The only problem is: What joint accel do we need to place the COP at a desired position?
But i guess you need only a hour for what takes me a week for this stuff


I made good progress. I can see my new controller can solve the fast weight transfer problem
It requires a single pendulum model for both feed and i need to improve the mapping,
but i'm in good hope i have walking ragdoll after weekend.

[Julio Jerez]

Ok I think I fix the Bug with et friction, when you have a change you can try again.
The handling of unilateral Rows, does take a cost.
the Ragdoll demo was taking 10ms for 100 dolls now I have to reduce to 50 for the same 10 ms.

by the benefit are worth it, with the limit be respected the fall all fall on Natural human poses. for me that's vey cool. all pose are natural looking so animation that skeleton should look pretty nice.

These actually cut the work on any of these papers almost half, they spend lot of time dealt with the problem that are a given for a physics solver.

The Pendulum stuff open a new Area of research for my. and the best is that it is no some crackpot model, it is actually use in real robotic research.

[JoeJ]

Now it's a lot better, but... still similar issues.
The T-pose ragdoll does not explode anymore, but it still makes sudden jumpy moves.
E. g. when it falls to the floor it kicks a leg in the air - no huge forces necessary.
This also happens when only one row has fricton, but less often.

It's hard and very rare to reproduce in your ragdoll demo, you'd need a powered joint keeping the setup pose, then it will happen all the time. (It would be also interesting how this affects performance - i can only create one ragdoll because of singletons. For me it's worth it if it only allows 5 ragdolls)

Good news is now it becomes visible that the skeleton can be used for human like natural motion.