Talk about inverse dynamics solver

[Julio Jerez]

I am making this thread for the people who are following the dynamics character controller.
It took few weeks, but I now re wrote the basic joint so that they can be reuse by the high level one.

the results so far seem even better than I expected.
I now have the advance robot, the setting is simple than before, but we had that,
the cool thing is that now I can place the walking Dog, and the limp are rock solid, yet they move.
for anyone following this is demo, ndQuadrupedRobot

at the top if we comment out a effector them the leg is disabled.
later I will add some control so that he can move the bodies and the leg, before I start playing an animation.

once we get this robot, working the best we can, and we learn the tricks, then we can move to the
inverted pendulums drive self-balancing biped.

so far it is going not too bad.

[Julio Jerez]

for anyone following this.
I now have the quadruped with a very simple walk gait
at first glance there is nothing special about it.

but what is special is that the leg takes a forward stride flexed about the stride back is a at full extension.
If anyone had seemed many of the robot you will notice that the knee and elbow angle always have a very large flex angle in olde to avoid the problem of ambiguous solutions. but that not how biped and legged creatures actually move, a human for example always keeps it limb at full extension because that the point of minimal potential energy.

anyway, before I move on there are two more steps to solve.
1- see the behavior when the effect moves off the swivel plane, the expectation is that it keep the plane.
2- animation the swivel plane, for turns and other kind of interesting moves.

after that is just about adding some ai to make the character move in the environment.

[JoeJ]

Haha, it already moves a leg.

2- animation the swivel plane, for turns and other kind of interesting moves.

Just wanted to ask how you could animate this, if all you can do is indirect control using effectors?
It's one example where i expected issues of 'lacking user control'.
But now i see you have a 'SetPositionAndSwivelAngle' function, taking both as parameters.

I guess your algorithm searches the next hinge ancestor of the effector, using this hinge to define the swivel plane?

The swivel angle always was quite an issue to me, because it's usually the open variable lacking an obvious goal, so it surely is good to expose it.
For the legs, usually we want to align the swivel plane to the direction of walking direction, but for human arms it's not so easy to say. It's why i came up with secondary objectives (limit energy to either reach or hold a pose, or bring the elbow to some target point when crawling, etc.).
In cases i also have a target orientation for the hand, not just a target position, reaching such orientation overwrites the secondary objectives if necessary.

[Julio Jerez]

Just wanted to ask how you could animate this, if all you can do is indirect control using effectors?
It's one example where i expected issues of 'lacking user control'.
But now i see you have a 'SetPositionAndSwivelAngle' function, taking both as parameters.

I guess your algorithm searches the next hinge ancestor of the effector, using this hinge to define the swivel plane?

The swivel angle always was quite an issue to me, because it's usually the open variable lacking an obvious goal, so it surely is good to expose it.

yes that's how people who do IK usually do it, that's know how I plan to do it.
we need a better way, I am still working on it, I will post soon and explain the algebra.
no point to do it now, since it might change.
but the trick is that base on the degree of freedom of the chain, we need to remove them from the equation

m0 = l * m1

where m0 and m1 are the pivot and L in the offset matrix.
for the industrial robot which has 6 independent dof we can have a full
three position component and three euler angle in the matrix.

but for something like and organic leg, there are only four dof, so some dof has to be function of other so that one is cancel.

btw I discovered that most robot implement 3 dof not 4
https://www.youtube.com/watch?v=Xp1F7Tm9534

if you look carefully the upper joint is implemented with a double hinge joint.
that's a far simpler problem, both mechanical and in software.

so in that sense the limbs are more like incest leg or robotic.
making 4 dof joint when the hip is a full ball, is not easy.
evolution has figure out how to do it quite easy for most mammals and birds.

[Julio Jerez]

Ok, you are right it is not eassy to control swivel angle with only four dof.

The effector is 6 dof, and finding literally relation between the position and angle to cancel two dof, is not trivial.

But it is not an impossible problem.
The way to do it it by fixing the leg more dof.
If we add a feet, a small sphere with disable collision, and attached to the calf with a double hinge.
Now the chain has two more extra dof fir a total of 12 fir each leg.
And the effector should calculate the exact angle.

I will try that before the last result.
Which would be to write the full equation of the chain and the solver it to find the exact rotation matrix. But that is an over kill.

[JoeJ]

Haha, watching that dog video a bit longer, it's really strange how asian cultures tend to explore a social aspect of using robots to replace pets. Robotic companions and friends? No thanks

if you look carefully the upper joint is implemented with a double hinge joint.
that's a far simpler problem, both mechanical and in software.

Yeah, i always thought they do this because it's super hard to build a spherical joint mechanically.
And making a robust motorized spherical joint with Newton was not easy either.
But i thought it's the better way. Otherwise you need more bodies and mass ratios increase.
Actually i never tried the double hinge idea.

If you use double hinges instead spherical joints, i see some potential advantage:
Double hinge can express natural joint limits better than a single cone. And if all your limits are simple hinges, your IK solver could do better planning respecting limits if turns out needed in the future.
If we wanted this also for spherical joints, we would need to agree on some convention on the limits.
Either an elliptical cone, which is a simple shape but lacks closed form solutions.
Or a solution like mine which is composed form 4 cones, which is a complex shape but has closed form solutions even for IK problems. (Recently i said it would't work because circle - cone intersection has no analytical solution, but turned out i don't need that. I only need simple sphere-sphere and circle-plane intersections to properly consider the swing limit for IK. But after that i still need to see if the twist limit has an analytical solution too, which i currently handle with iteratve approximation. And i hate iterations.)

I still think it's fine to ignore limits for IK and then enforcing them afterwards (that's what i call 'sliding at the limits').
But at some point it may be no longer good enough for complex problems. We'll see.

[Julio Jerez]

ok Joe.
what have I done? it's alive, it's alive!!!. https://www.youtube.com/watch?v=QuoKNZjr8_U
First test far surpasses my best expectations, it is monstrous!!!

if you check now, you will that the new joint picks up the extra rotation calculated by the solver and the leg follows very close the effector. You can see how the axis move in the double hinge.
It is those minor changes of rotation that introduce the nonlinearity in the 4 dof joint chain and is hard to calculate but geometric algebraic manipulation of the effector matrix, but they are linear in a 6 dof space. check how the twist axis of the thigh wiggles as the effect move in a staring line forward and back, that's the error we see that is extreme, and now I argue impossible to get right form a simple manipulation of the effector transform, the acceleration seem to follow some wave which would be typical from some a derivative of a oscillation pin. but anyway, not need to get hung up there anymore, we have a much more powerful method now.

these results are so good that from the side view it is as if is moving on the plane of the ball joint, but when look from the sagittal plane of the dog, you will see that the effector is 0.2 units off the plane.
So far, I am liking these results.

Notice that this is not cheat in anyway. That extract node is just a mathematical transformation to get the correct solution for the joint locations. A real robot will not have those elements and will still work perfectly.

I placed limit on each joint just to see them, but in reality, they do not need limits.
One of the really nice features of the method is that limits are not as important as they were before.
In fact, I removed many limits from the industrial robot. This is because a good effector controller will not place the effector outside of its workspace. It does not solve the problem when location has multiple solutions, but now the joint limits are used to make it so that on those cases, the joint limit selects the prefer solution therefore the only joints that need limits are those where a solution is undesired.


I tested that before, but I have to do some stress test on the Dog.
We are now getting very close my friend.

[Julio Jerez]

ok moving on, I now let the dog loose in the environment and very quickly learn that an extreme naive walk gait does not do it, it looks more like a generic AI trying to learn to walk.
but all in all not too bad. the limb seems to be rock solid.

I was looking and how gait for quadruped work, and according to the video, a walk set legs at 0.25 phase angle.
https://www.youtube.com/watch?v=tLrRlXxM5Yw&t=37s

the way I made the walk stride and look like the firs image and has to be more like the secund image, so I have to make that over the weekend.

Untitled.png (4.34 KiB) Viewed 10775 times

This is so that there are always three feet supporting the body at the same high.
I am animating it very slowly so that we can see what is going on.
but as I say is not bad at all.

[JoeJ]

I tried to make it walk forward by syncing diagonal legs motion:

Code: Select all

dQuadrupedLeg* const effector = new dQuadrupedLeg(effectorFrame, pivotFrame, childBody, m_rootBody, /*definition.m_walkPhase*/((index&1) ? 0 : 3.14/2));
m_effectors.PushBack(effector);
index++;


Did not help. It still acts clumsy.

But it's aaaalive!!!

[Julio Jerez]

yes, but that 's because the model does not has the correct gait path. with eth sin wave, only one leg is touching the ground plane at any given time, so it is spending mor the energy in falling. but making 180 offset we get two, but now is just going up and down. that idea is that is always has three contact to support the body at all times. That's for a walk or for a stance.
do no worry we will get there tonight I believe.

this is actually quite a good test, because it shows the robustness of the system.
whet come next is a state machine that on each state we model a controller type.

for example the walk, could be procedural, (that the one I will made first)
but we can make adaptation that use an inverted pendulum for predict the position of the foot, in fact I can see easily that the walk cycle be made but just the kinematic motion of an inverted pendulum fitted to each leg.

another way to go is that we can train a walking robot using Reinforcement learning, it is my contention that if we use a Deep learning reinforment learning on system like this, the training time should be reduced by more than one order of magnitude.
The reason I say this is because on this system the input space is far less noisy.
In all the system I seem, the deep leaning neral net spend most the time tunning joint toque, which is extremally noisy. plus they also has to make up for the bad physics engines, when the ai had to secund guess the physics.
In this system the imput are the three angles of the effector for each foot. really three, since is reduce to a position on a plane and a swivel angle, (but the machine can figure that)
and the output is the balance and the maximization for the Goal.

I am sure you have seeing some of the funny animation generated by tensor flow and OpenAI, they are touted as great success and I see them quite mediocre.
It is my contention that if the physics model is obeying the laws of physics as much as possible, that the outcome should be close to what nature produces. rather that this *.
https://www.youtube.com/watch?v=gn4nRCC9TwQ&t=45s
https://www.youtube.com/watch?v=2cjkKnAxCug

on that training that AI exploited a severe flaw of the physics engine, which is that the engine does not conserve momentum, so if he throws the arm forward, there rest of the body react very little because the momentum reactions vanish on the solver soft joints. so the reinforment reward policy gives a reward for that action every time, so even in a long or short sequence it always gets that reword con conclude that those are legal valid motions.

I expect a far, far better behavior than that.

[JoeJ]

Oh, video titled 'Elon Musks new... this and that.'
And then saying 'Looks goofy, but it's smarter than you think'. Just believe it! Hahaha
I'm sure you'll do better...

[Julio Jerez]

ok Joe check this out.
This is where the physic keeps throwing all those wonderful monkey wrenches to the mix.

the model nod is a convex hull, that has more volume in the back that it does in the front. so we expect the center of mass to be bias to the rear of the model. and it does.

now what these causes is that the model is back heavy. but still when the four leg are plated of the floor the com lies inside those four points.

we would expect that if he lifts one leg and the com lie inside the triangle, that the model should remain balances. But guess what is does not.

these what happens, when eth model lift one leg, the weight of that leg now shift the com toward that direction, making it lie outside the support triangle. so if the com we too close to taht edge of the triangle, the model would topple over. that is the example correct behavior that is expected.

so right there the biomechanical properties of the model impose that this action need to be balanced with a shifting of the hips to the opposite side, just so that the com lie on the support triangle.

I check in the model lift one left leg so that you can see the effect. if it is lifting a front leg the problem does no happens, but that because the com does no crosses the line when the support is lose.
however, lighting the same leg faster, causes the body to topple because the conservation of momentum, teh action to move their leg up faster cause an opposite reaction on eth upper body.

what this means is that the sway of the hips is a function of the weight distribution.

this is why I say that if we train these models with a reinforcement learning algorithm, I expect a far, far more plausible natural motion because of the physics of the model, so the neural net will reward the action which are deterministic. instead of learning how to go around of bad physics.

now I can see why nature favor 6 and 8 leg in the insect world, a small brain does not have to coupe with that, but still has to move.
I did not expect that weight shifting to show so early.
I will do some procedural without calculation and her is whet the spherical joint comes handy.

the way the real robot deal with that is the legs are in fact real close together, so lifting one leg does no changes the support shape by too much, I made the model wide think that this would not happens, and got the exact opposite result, so again nature favors skinny body frame.

[JoeJ]

the way the real robot deal with that is the legs are in fact real close together, so lifting one leg does no changes the support shape by too much

Though, that gives a smaller support polygon, which makes balancing harder than just shifting the weight.
I'm sure they do precise weight shifting even for quadruped robots like Spot. It's the only way to get natural agility.
The primary problem is to do such weight shifting at the correct speed, so maintaining balance is guaranteed also in the future, and the motion also happens as fast as possible within this constraint.
The weight shifting movement has a acceleration and deceleration phase, and personally i plan both of this ahead of time at every moment.
That's why i need precise control of COM, and effectors alone don't give me such control.
So you need it at least internally of course.
For the biped this process is very sensible. This is where Newton displacing a contact to the middle of the foot for just one frame caused my ragdoll to tip over.

Eventually you need shorter (or lighter) legs to make some naive walk cycle while ignoring this stuff.

[Julio Jerez]

That's why i need precise control of COM, and effectors alone don't give me such control.
So you need it at least internally of course.

I completely disagree there. but we will see.
I do agree that the motion do need easy in easy out acceleration, but that's why the effector, my contention is that using effector is a much better way to do that because the effectors motion is linear and they can by seeing as the independent variable of a non linear system, which is the solver job.
But I know I am probably the only person on earth with that idea, and I need to prove it. Every one seems to think that calculation joints torques with pd is better, and to me that's not just harder, it is simply incorrect since those torques are not independent random variable, they are deep cross correlated to other torques and forces, while joints acceleration are true independent variables.

I now made the part the lighter only the front leg, knowing that the weigh is bias to the rear,
the vertical leg motion has an overlapping period of one frame.

now as I increase the frequency of the animation, you can see that at some point lifting a leg brings the body down a little, the variable that control the speed is and at about 40 start doing that

ndFloat32 frequency = 40.0f;

motion. some people call that concept zero moment point, which is a point where the moment on the com is zero considering the momentum, but we will deal with that in a different way.

anyway, tomorrow I will add the weigh shifting modification and we will see how that goes.
the motion start to look good now.

[Julio Jerez]

now spot is taking his first baby steps, not a great success but no bad a s prove of concept.
I will try some tweak, but it is clean I need a far better-quality animation.

but all in all it is surprising that it even manages to take few steps.