Talk about inverse dynamics solver

[Julio Jerez]

ah more development,
to hack the effect of a PID, I just added a fake scale that amplify the error.

there is a very good intuitive explanation of the PID controller in wikipedia
https://en.wikipedia.org/wiki/PID_controller

for us as I said before, the correction should be

Code: Select all

accel = - ke * error - Ki * errorSum - kv * veloc;
errorSum = errorSum + error;


the hack is just scaling the error, in essence making a stronger Ks, while will be wrong, but for testing is ok.

if you now just go to line 248 in file ../newton-4.00\sdk\dNewton\dIkSolver\ndIk6DofEffector.cpp

and you play FAKE_PID_factor, you will see how the yellow dot catches better with the target.
and that is when we look for, by is had to be adaptive which is what the PID will do for us.

float FAKE_PID_factor = 10.0f;

our Implementation of the PID will be like this

Code: Select all

accel = - ke * (positError + errorSum) - kv * veloc;
errorSum = errorSum + Ki * positError;


this way the ki pid will be a normalized scale of the error. if we make it zero, it will be them is will be and normal PD as it is now. no all joint need PID.


and finally if you go to the controller, and uncomement the line that set the target matrix to be the desire one. lie this.

Code: Select all

      ndVector localGravity(matrix1.UnrotateVector(m_gravityDir));
      //ndMatrix targetMatrix(m_effector->GetOffsetMatrix());
      ndMatrix targetMatrix(dGetIdentityMatrix() * matrix1.Inverse());
      targetMatrix.m_posit = localGravity.Scale(1.0f) | ndVector::m_wOne;


it just moves to the point that keep the balance, applyng that toque to the box, and them just stops there.
the top box keeps rotating because is reach and indifferent equilibrium, it is exactly the behavior we look for, basically the rotation of the box does no add or remove momentum to the system for that the pivot has to be offset from the center so that any rotation make the com goes up of down.

so far I like these results.

[JoeJ]

I lag one version behind.
But first, i wanted to visualize com, which did not work:

Code: Select all

void Debug(ndConstraintDebugCallback& context) const
   {
      ndJointBilateralConstraint* const joint = m_effector;
      joint->DebugJoint(context);
      ndMatrix rootMatrix(dGetIdentityMatrix());
      rootMatrix.m_posit = CalculateCenterOfMass();
      context.DrawFrame(rootMatrix);

      ndVector com = CalculateCenterOfMass();
      context.DrawPoint(com, ndVector(1,0,0,1)); // want red dot, but does not work
   }


And i could not find your effector visualization code with the yellow dot.

I'm a very visual coder, so i want to learn how to add it myself to your demos.
Personally a have a global debug singleton to record drawn primitives, so i can add visualization code anywhere and it just works.
What do i need to know to draw stuff from your demo framework?

[Julio Jerez]

Do you see a vertical line passing thought the com.
And a small yeow ball?
The last commit should show that debug info.

In the code you show, the com will probably be inside the big box.
You can set it to hide visual mesh and enable display debug collision hidden line removed.

[JoeJ]

In the code you show, the com will probably be inside the big box.
You can set it to hide visual mesh and enable display debug collision hidden line removed.

I tried this already, but still no dot.
Will try again with the update...

[JoeJ]

oh, i guess com is returned in some local space, and 1 for color is still black...
i'm figuring out...

[Julio Jerez]

not the com is in global space.
if you sync now, it will show it in wire frame, and you will see the lines.

them you can check this line to see the difference

// specify foot orientation.
targetMatrix = dGetIdentityMatrix() * matrix1.Inverse();

edit:
if you sync again, I am drawing the center of mass a s big red dot. you will see that it is inside the box box.

[JoeJ]

Ok thanks, i see your alpha should be 0, not 1 as assumed.
Interestingly the com was calculated to the worlds center (i guess), and did not move with the model before the update, but now the same code works es expected.

[JoeJ]

them you can check this line to see the difference
// specify foot orientation.
targetMatrix = dGetIdentityMatrix() * matrix1.Inverse();

i did this:

Code: Select all

ndVector localGravity(matrix1.UnrotateVector(m_gravityDir));
//ndMatrix targetMatrix(m_effector->GetOffsetMatrix());
ndMatrix targetMatrix(dGetIdentityMatrix() * matrix1.Inverse());
targetMatrix.m_posit = localGravity.Scale(1.0f) | ndVector::m_wOne;


Now the ball isn't turning. And the box tilts slightly to the right.
It is spinning and sticks at this right side position.
Why doesn't it fall down?
It is spinning, so maybe a flywheel. But it turns to wrong direction. The spin would only accelerate the fall.
Maybe the spin is so fast it only looks like going wrong, but actually creates the flywheel to make it levitating.

Now you need to explain this...

[Julio Jerez]

no is no flywheel, the box has symmetry in all three axis, so the inertia is spherical.
the you see the yellow box tow frame behind because the display does not work in sub steps.
but reach the offset where the calculation of the force one step in the future aligmened with the projection to the com. that task is to reduce that gap. hence the PID
the ball will not move because the net force and the momentum is near zero. this is correct.

now if you play with that line that force the orientation you can see the difference between keep rolling and stops.

that you can play with this line float FAKE_PID_factor = 10.0f;

1.0 is a weaker PD,
10.0 is more or less what a critically balance PID will be,

I try 20, and is become unbalance and the yellow point will move pass the com line and the com back. at that point the test is no longer valid, because we are making a PID with just a PD with a higher.
spring constant.

I know have to finish the actual PID and do the same for rotation. once that is in place the box should just move the target orientation and just stop. at least that is what I expect to happens.

[Julio Jerez]

note:
if you try to move it around it will blow up because the fake spring factor, so do not even try it, that is no part of the experiment.

[JoeJ]

x.jpg (12.03 KiB) Viewed 9589 times

But no - i wonder again how it can stay at the state as shown in the picture, without falling to the right?
That's the same i saw few days ago and thought it's a bug.
It defies the laws of physics?
It's not possible form internal forces, so there must be unintended external forces at play?

[Julio Jerez]

yes that is correct. that's a typical response of a PD controller.

is teh equation a

a = ks (x1 - x0) - kv * (v1 - v0)

when v1 - v0 = 0

the secund part is zero,

that leaf ks * (x1 - x0)

x1 - x0
has to be different than zero for there to be a non zero acceleration.

in the image that you posted imagine the top box.
it is pivoted at center of the small box, so gravity will add a torque that will pitch it down.
the joint has an acceleration that will try to move to the line that show the center of mass, so it will generate a torque on the top box in the opposite direction and they will cancer each other.
the vertical force is also cancelled by the joint pointing up. so the big bug and that gap, is in equelibrium. because when the solver calculate all those forces and torques, they amount to zero.
the big ball roation does no generate torques, so it keeps spining.

the free body diagram for the small there.
the liner rows do no add toques because they are applied at the center of mass.

the angular row generated the some torque that was applied to the top box in the opposite direction.
by that box is constrained by friction, so it too get to an equilibrium point.

it is hard to see visually but I solve for a simpler test on paper in two dimension and there are infinite number of equilibrium.

so in order to move forward we need to add an active action that close the gap. this which I am tri to add the PID, but so far is not working.
it seems that one way is to make it an actuator.

the gap is a function of how strong the spring action is, which is an indication that make it need an active motor. one that does not stops, until the error is a numerical zero.

but the joint is working, even if it does not look so.

[Julio Jerez]

remember this image.

Untitled.png (23.77 KiB) Viewed 9585 times

you are thinking that the only equilibrium point is when the ball is at the apex.
but now imagine the ball fall to the right, and the is a gush of wind that is just strong enough to cancer the gravity component bringing it down, the ball will stop a some point below the fix point at the apex.
it is possible to stop the ball at many different points alone the trajectory.
so for the ball to go back to the top, it needs a stronger wind that generates a counter force stronger than the one to keep it at equilibrium, and a PD controller can't do that.
So it needs and active motor to close that gap.

[JoeJ]

you are thinking that the only equilibrium point is when the ball is at the apex.
but now imagine the ball fall to the right, and the is a gush of wind that is just strong enough to cancer the gravity component bringing it down, the ball will stop a some point below the fix point at the apex.

Yes, but the joint can not generate a torque which would match such wind. No way.
If the small balls orientation would be fixed to the world space, then it could.
But there is no such fixed link. The box must fall down.
The only way to prevent the fall for the joint would be to move the ball to the right, resembling to make a step.

If anyone else is reading this, please help me out! Julio must be stopped!
Tell us what you think guys!

Ok. Example task: Stand upright, lean forward so your com projection is in front of your toes.
You are not allowed to move your feet forward. But you can protect your nose. It will hurt.
Not even superman feet could prevent falling on his nose.

Show this example to your wife, or anybody. No equations. No education on your side. Hear what they say. This is not some trick experiment physics professors use to baffle their students.
It's obviously wrong!!! You're cheating!

You did paste in my snippet and saw yourself? Misconception totally excluded?
And to be sure - your joint allows to rotate the (invisible) stick which has the ball on its end?

[JoeJ]

Looking at the code, i don't see a joint between box and ball at all. There is just the effector?
Then maybe my assumption is the problem.
So which internal forces exactly keep the box in air?

Edit: I see it's the IK6Dof which generates the link between the two bodies, and also is the effector at the same time.
I was confused because i thought you'd use the effector in addition, basically to 'pull' the ball, but the effector would not be part of the skeleton itself.

So i guess you have a bug in the 6Dof, similar to the limitations i experienced here:

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.

But the better example is the 'super joint' i tried to made. It should both rotate and position a child body arbitrarily. It did not work and behaved not correctly. External force behavior, so i could not use it.
Pretty sure that's the same issue. 6Dof sounds like 3 degrees of rotation AND 3 degrees of translation. And in my experience Newton can't do this correctly.

If i'm wrong i'll eat a broomstick and pay extra beer, but please ask for some others opinions.