Development of self balancing biped with inverse Dynamics

[MeltingPlastic]

Yeah I think that's what I mean. I am getting more into robotics. I would actually think that exposing the skeleton from any ndJoint or body would be fine too.

[Julio Jerez]

How would one get the forces needed to create a specified linear and rotational acceleration?

Thinking about that feature some more, and U think you are upto something that could be very useful.

In 3.14 some of that already exist, the is a application size duplication of the Skeleton, but it was never practical since it was different. I wouldn't recommend using that,

That why we now have 4.0 with models.

Here is a problem I aways have difficulties dealing with.

Say I make a Robot like a Kuka.
One of the biggest problem is to find the work space of the effector. So I alway had to be very careful when to move it.

The work space of the effecto is hard to determine. And one of the worse things g you want in a system is hitting limit.

The reason for that, is that in the matrix equation I posted, when a joi t hit a limit, in the matrix this means the on of the f is either f0 or f1.
And at that point the system lose one degree of freedom.

In the case of a robot, say two hinges, if one hinge reach the max angle, the the second hi g can only move is a cicle.

In these cases the user, is not aware, a d try to force the limit, cading even more violent response a d vibration.

Imagine now that we put the robot in a ndModel.

In the update we get the Skelton, a d we set the desired target position,
Them we call the immediate solver to get the forces,

Now we get a force vector with the joint reactions,
We inspect this reaction an if one hit a limit, or is close enoght. We can change the target position, in such way the the limit will not be broken.

Another cituation, would be preventing double undesired solutions.
Imagine a knee joint, whe the cold is flexes it is fine, but is just happens that the most common, position is what the leg is straight.

And the the kneed can easily bend forward or backward.

With this new tool, we can find if it hit the forward limit, and change the configuration such the the two links are fused but a fix joint.
Oh mine

[Julio Jerez]

Yeah I think that's what I mean. I am getting more into robotics. I would actually think that exposing the skeleton from any ndJoint or body would be fine too.

No, you do not work with joints. That's a hunge mistake,

The isdia is to work with constrations of bodies, joints and contract as a monolithic entity.

The issue I described. Is particularly hard when trying to manipulate a robot gripper.

But with this new funtionality, it is eassy to measure the gripper force, and adjust it appropriately.
This sounds like a gold mine of functionality my friend.
That's the point of the model.

If I were, you I would spend few days migrating to 4.0
It is way easier to used than 3.14 and at this point more features more robust, faster and more stable.

I will try to work on that feature for the ND model next, and see if I can name a better Robot demo.

[Julio Jerez]

ok to get the ball rolling, I added the interface to get the skeleton from a model.
it will look like this.

Code: Select all

class ndModel: public ndClassAlloc
{
   public:
   ndModel();
   virtual ~ndModel ();
....   
   virtual ndSkeletonContainer* GetSkeleton() const;
}


It does not anything yet , I have to add few functions to the Skeleton that can be call on the returned pointer.
Notice that each body associated with a skeleton, has the pointer to that skeleton in it.

so It seems that, you could get the skeleton everywhere. But that is not true. if you do that, you make get an invalid pointer.
Just like contact joints that are generated and destroyed in each simulation step, the same happens to Skeleton, they change configuration as the get in contact with other bodies and other joints.

this is why there is a ndModel Update,

ndModel update is just a new object we introduced since 3.xx but now they are part of the core engine.

if is quite eassy to make a model so that you can get update.

all you need to do in subclass from ndModel,
and add a members to hold the bodies that will be the representative of you articulated model. You registe the model just like you do with joint and bodies, and now you have and update and post update method, that let you operate on all those object after the contacts, and Skelton are executed, but before the solver step.

the function GetSkeleton would be some like this

Code: Select all

inline ndSkeletonContainer* ndMultiBodyVehicle::GetSkeleton() const
{
   return m_chassis ? m_chassis->GetSkeleton() : nullptr;
}


however, once you have that object, you can now add vectors that will save all the bodies, and the relevant joints. for example, say you make a rag doll,

you could same just the pelvis body,
but you can also save the power joints, the end effectors, and so on.
this will be more clear after I made the inverse dynamics Robotic demo sometime this week.

[MeltingPlastic]

What happens when 2 models start to interact with each other with contact joints? Will the 2 models return the same skeleton since they are connected? I ask because I made a game that the players could connect 2 Models together at run time by creating new joints. So what was once 2 models would be 1 model.

[MeltingPlastic]

Another example would be 1 robot picking up another robot

[Julio Jerez]

If they are interacting with via contact they get different Skeleton each.

If you connect them with a bilateral joint, them the will get the same Skeleton.

At the point, it is a seven management problem on the app side.

The app can chose do nothing, in which case the not sure what will happen, since the update is multithreaded, so two or more Skelton can be solve simultanueslly, but not the same Skelton twice. I suppose we can add a mutex. So that if it two model try to solve the same Skelton, they get serialize.

The more partial case would be say two vehicles, one is atrailer, and the get linked.

The app cam remove the Skelton from the word, and kept the in some data base, the from make a new model from an array of model and that will be the new models.

That's what the engine dos with many object. The app is in better position to do that better.

In the snad box, the vehicles are models, and you can driver on vehicle one another, and it work just find.

This is because the to the Skelton, 100% close loop joint are break point, similar to what static bodies are to island,

They get solve indendently, the the open loop, which get is a joint with two bodies, only the link to the Skelton get on on force, the other is untouched.

A close loop can connect to two bodies on the same Skelton, in which case both force are calculated and they just happens to be the same, so average is the same,

Is one body is on a Skelton. One force is calculated, the other is calculated be the generic solver, and they are average.

If one body is in one Skelton and another in another Skelton
Each skel calculate the new force. And those are accurate.

Remember sleyon are just operations. Very much the same way a 4x4 matrix class has operator that do stuff like matrix inverse, cross product, etc.
The Skeleton
Is just a very optimized matrix solvers that

Solve the system.

A x = b

Where x0 < × < ×1

Here x represent force and b acceleration generated by the joints.

So the function on the Skelton will be something like solve

And the app pass a vector to the output, the it will return, the
Array of joints, the jacobian, the forces, the accelations, and the joint limits.

With that vector now you have everything forwar and back dynamic to do what ever you want.

[JoeJ]

I do not precisely understand what you guys talk about, but maybe i have a similar request.
What i want is to give a target pose to Newton, which then tries to reach it.
Though, i already can do just that using the old joint motors. It's how my old ragdoll stuff worked - I did calculate motor joint accelerations from the pose.

But it always sounds like Julio has some concerns about such method.
However, i also have concerns about the actual new active Newton ragdoll. Summing them up again:
The system is meant to take effector targets, and do all the rest on its own, including balance and the IK problem to find a pose? (Not sure if this is still the plan.)
But that's not what i want. I want to control the entire pose on my side. For such reasons:
Precise control of COM is required, otherwise i can't keep the ragdoll in balance. But COM is no effector or even bone - i can control it only indirectly by providing full body pose.
Precise control of pose is required, because that's about artistic expression and game design, which is beyond of what a library can do for me.

That just said to show my background, avoiding tech or math talk, which often ends up more confusing than it should be.
All of the above are assumptions. Currently i'm not sure if i can / should use the new ragdoll system, but i'll give it a try and only after that i can give proper feedback.

Beside option to give my full pose to the new ragdoll system (can i do this?),
i see another potential usecase for the feature in discussion:
Could we use Newton as a ragdoll solver only, for some robotic character system middleware targeting engines which already have their own physics stuff and won't change this?
Something like Natural Motion. They used their own torque solvers for ragdoll joints, which then was fed into Havok joints for example (afaik).

[Julio Jerez]

What i want is to give a target pose to Newton, which then tries to reach it.
Though, i already can do just that using the old joint motors. It's how my old ragdoll stuff worked - I did calculate motor joint accelerations from the pose.

in an ideal world that works. but in practice, joint hit limits as they move.
and one a joint hit a limit it is too late for making a correction.

I would be nice if the app can predict the what would happen under certain conditions, so that the controller can make corrections.

at the end of the day, the force that act in a body and the reaction forces form a liear equation of teh form

m * a = fext + x * J
J dot a = - j' * v - penalty.

penalty is the time derivative of the velocity at the contact point which we model as zero for rigid joints, or some implicit stiff stpring damper, and a constant penetratoon recovery speed for contacts.

a you can see J, m, fext, are constant
a and x are the unknown.

that's the system of equation that the engine solve every frame.
when you substitute one the other you get one linear system of equation of the from

A x = b

where x is the same as equation 1,
A is form for a combination of the m, J and inertias of the bodies and the geometric configurations,
b is the combined acceleration and penalties.

now no all the x are valid so the equation is hard to solve because some of the x has limits.
so the augmented equation is

A x = b
x0 < x < x1

where x0 and x1 are the limits of the forces.

now the engine always solves for x because the acceleraion are given by the geometry, but every time the xs are calculated they are applied to the system so the so they is no time to make correction before they happens.

now imagine you can do a test,
say

x = inv (A) * b

but these xs are not applied to the system.

now you have a vector with all the accelerations, all the forces, all the forces limits, and all teh Jacobians the solver use and also all the joint.

with this information, know the logic can see many things.

I give you the simpler example that I can thong off.

imagine an leg

the knee can ben forward and backward. it just happens that the rest position is always at the boundary where the knee should not vent backward.

now if you want to take a step back, that is always fine because the knee vent forward. but of you want to take a step forward, now the knee vent backward, but that hit the limit making bounce back, and generate a horrendous jitter.

now imagine we can try the solution an get the forces.
the logic now can see that the knee joint x hit the front limit.
so the logic can say, on the know I will apply a motor that forces the knee to rotate back.

another example would be the way people in the robotic community control robots.

the like to use engines that use generalize coordinate because those system deal with forces and torque, and these value map well to motors.

imagine a robot that you want to determine the torque, of the voltage, of the current you need to apply for reach a target. you can set the target, call for eh solver and get the forces and torque for all motors. now the application can simply apply those values to the physics device.

the way is set now is that you set the target solve and get the forces, as a result robotic app that use physics engine ignore the powerful motor joints, because the cannot get the result the want before the solve step.

that function can provide that solution.
I can see so may application to having and efficient functionality like that
I hope you see more clear.

[Julio Jerez]

Code: Select all

The system is meant to take effector targets, and do all the rest on its own, including balance and the IK problem to find a pose? (Not sure if this is still the plan.)

that is the plan, but it is postponed until I get these basic core stuff out.
when I resume, I will be making a heavy use of the functionality.

like you say, this could be used to calculate forces, in a close system, then disable the close loop and fee the forces to the articulated body, which is think is more of less what system like natural motion are doing.

[Julio Jerez]

So basically Joe, and melting plastic, is is as simple as this.

You assembly the articulation with end effector at the tip.
Them in the pre step, you move the effector to a desire position, and you call the function to calculate the torque.

With the force and the jacobian and you calculate the external force that need to be applied to each body, to achieve that desired motion. And you disable the effectors

Here is the simplest example I can think off.

Imagine a hinge. That is free to rotate.

You want to know what torque to apply on the hinge axis, to move to certain angle.

Of course the hinge motor does that, but that's a change of position. And what we want is the torque.

We set the motor. And in the ore step we calculate tge force.

After the call we now know the force,
So what this means is that if the hinge was not a joint motor, and instead we apply and external torque to the free hinge. That is tge Jacobian multiply by the torque, them the hinge dies not need the motor.

Now you extend that to more comple arrangement, and this is what robotic seek, because they one forces and torques, not acceleration.

[JoeJ]

in an ideal world that works. but in practice, joint hit limits as they move.
and one a joint hit a limit it is too late for making a correction.

In my example, moving from existing pose to target pose is guaranteed to not violate joint limits. I calculate the target pose with IK solver which already respects them.
I remember i tried to use tighter limits for the IK solver, so we never slide at the true limits. I did this to see if Newton runs better when not touching the limits. But i could not really answer the question, because while doing basic locomotion we never really get close to the limits anyway.

Back the day my IK solver was simple and trivial. It did just slide on the limits, and at this point it likely failed to hit the target in one step. In the next step it may slide closer to the target and so on, you can imagine. This is good enough for basic locomotion, but inefficient for other tasks our bodies are not primarily built for. E.g. aiming a weapon can require more advanced planning respecting the limits, to avoid looking dumb while sliding on a limit and not reaching the target quickly.

I worked on this recently and saw it's quite difficult. It involves problems lacking analytical solutions, e.g. calculating intersection of a circle and a cone. So i need iterations to search for a solution, which makes it more expensive.
Even worse - the clever IK solver is now more likely to flip between two very different solutions which both could hit the target, but the space in between them is blocked by limits.
Such flipping issues are surely one of the main problems we have to address. At the moment i'm not yet sure the 'clever' IK solver will be a real improvement.

You assembly the articulation with end effector at the tip.
Them in the pre step, you move the effector to a desire position, and you call the function to calculate the torque.

[/quote]
This gives me all the doubts.
For a ragdoll, we have effectors at the hand and feet, and at the head.
But we lack any control of internal bodies, like belly and hip.
Imagine we have a dancing animation, and we feed Newton just with it's effectors.
No way it will re-imagine the dance for us. In the best case the result will look like natural and valid motion, but the dance itself, or at least its expression will be lost.
I think you need to allow control for internal bodies too, eventually in form of secondary objectives, so trying to get close but not guaranteeing to hit them as precisely as effectors.

But we will see. Every idea should be explored.
Currently i see absolutely no other way for games to move forwards than smart robotic characters.

[Julio Jerez]

I do not agree that using effectors can not recreate natural looking motion. I put it is just the opposit.

In fact I go beyond, using rig of effectors is the way to make natural or unnatural motion.

Tech artist spends most thier time rigging skeltons with all King of dependent effectors, so that the can animate complex Skeleton in an easy way.

Only very rare a simply Skeletons are animated using forward kinematics.

The difference is the rigs make by animators,, are really complex and gas lot of redundancy, and are older the supervision of a person, so that help.

The point I try to solve is to have a way to know in advance the forces needed for a pose.
This reduces the uncertainty of multiple viable solution, and provide a way for simulation of real systems that use motors,

But you are right if you use a ik system system to predict a target, them that similar are to what we are try to do, just the IK do not provide the forces.
Having the force in advance is a big issue in robotic.

There is a new ik system called Fabrick, that is really fast, and seem very flexible. It is what most game engines are using these days, I was thinking of using it for the prediction.

But I think I like better the force matrix inverse solver better
For the reasons I stated before

[Julio Jerez]

ah Joe, I was looking at that functionality and I am more and more convinced that will be a really nice functionality to have, not only for th evenefix of some app, but it will be good all around.
check this out.

the is in general the pseudo code of the solver

Code: Select all

void ndDynamicsUpdate::CalculateForces()
{
   D_TRACKTIME();
   if (m_world->GetScene()->GetActiveContactArray().GetCount())
   {
      m_firstPassCoef = ndFloat32(0.0f);
      if (m_world->m_skeletonList.GetCount())
      {
         InitSkeletons();
      }

      for (ndInt32 step = 0; step < 4; step++)
      {
         CalculateJointsAcceleration();
         CalculateJointsForce();
         if (m_world->m_skeletonList.GetCount())
         {
            UpdateSkeletons();
         }
         IntegrateBodiesVelocity();
      }
      UpdateForceFeedback();
   }
}

as you can see it is a loop that iterate for time, the four comes from the RK4 integration.
m_firstPassCoef is to indicate the joints when coefficients to use, stat at zero and increate after each call.

the last line is when the solve collect the feed bas to the application, mainly the forces added by each joints, the net forces and the net accelerations.

that function is not really necessary for the solve operation, and in previous versions, it was an option in the joints. but that was inconsistent, since setting on one joint and no other report incorrect net.
even when setting to all joint, if a joint body collide, then the same problem.

so by late 3.14 I made an unconditional part of the solver.

but as you can, the happens after the solver calculate the forces.
Therefore it is of very little used for many app, plus the function is not cheap. this is how is implemented

Code: Select all

void ndDynamicsUpdate::UpdateForceFeedback()
{
   D_TRACKTIME();
   ndScene* const scene = m_world->GetScene();
   const ndArray<ndConstraint*>& jointArray = scene->GetActiveContactArray();

   auto UpdateForceFeedback = ndMakeObject::ndFunction([this, &jointArray](ndInt32 threadIndex, ndInt32 threadCount)
   {
      D_TRACKTIME();
      ndArray<ndRightHandSide>& rightHandSide = m_rightHandSide;
      const ndArray<ndLeftHandSide>& leftHandSide = m_leftHandSide;

      const ndVector zero(ndVector::m_zero);
      const ndFloat32 timestepRK = GetTimestepRK();
      const ndStartEnd startEnd(jointArray.GetCount(), threadIndex, threadCount);
      for (ndInt32 i = startEnd.m_start; i < startEnd.m_end; ++i)
      {
         ndConstraint* const joint = jointArray[i];
         const ndInt32 rows = joint->m_rowCount;
         const ndInt32 first = joint->m_rowStart;

         for (ndInt32 j = 0; j < rows; ++j)
         {
            const ndRightHandSide* const rhs = &rightHandSide[j + first];
            dAssert(dCheckFloat(rhs->m_force));
            rhs->m_jointFeebackForce->Push(rhs->m_force);
            rhs->m_jointFeebackForce->m_force = rhs->m_force;
            rhs->m_jointFeebackForce->m_impact = rhs->m_maxImpact * timestepRK;
         }

         if (joint->GetAsBilateral())
         {
            ndVector force0(zero);
            ndVector force1(zero);
            ndVector torque0(zero);
            ndVector torque1(zero);

            for (ndInt32 j = 0; j < rows; ++j)
            {
               const ndRightHandSide* const rhs = &rightHandSide[j + first];
               const ndLeftHandSide* const lhs = &leftHandSide[j + first];
               const ndVector f(rhs->m_force);
               force0 += lhs->m_Jt.m_jacobianM0.m_linear * f;
               torque0 += lhs->m_Jt.m_jacobianM0.m_angular * f;
               force1 += lhs->m_Jt.m_jacobianM1.m_linear * f;
               torque1 += lhs->m_Jt.m_jacobianM1.m_angular * f;
            }
            ndJointBilateralConstraint* const bilateral = (ndJointBilateralConstraint*)joint;
            bilateral->m_forceBody0 = force0;
            bilateral->m_torqueBody0 = torque0;
            bilateral->m_forceBody1 = force1;
            bilateral->m_torqueBody1 = torque1;
         }
      }
   });

   scene->ParallelExecute(UpdateForceFeedback);
}


the last loop is the part that calculate the forces added but the joint to each body.

but like I said, the make each joint to have four vectors to tow for forces and tow for torque to carry around those values, the almost no application every need for.
I can only think of one app that ever use was not completed satisfactory and the end using PD string damper forces anyway.

now, if we can that feature, we only need the for loop that safe the force scalar because they are the initial guess for next step. but the secund loop is not needed, so it gains performance.
the joints do not need to have the extra vectors. so it saves memory

and most important if an app need the forces, they can get it before the solve step, and only for the joint that want to.
those are extreme strong reasons for that feature.

[JoeJ]

Sounds good.
IIRC, i had used the torque of joints only to verify my predictions match simulation, but then i guessed given torque is not very accurate, and you said it's something like an initial guess.

I also thought the feature to calculate joint forces to reach a pose could be useful for controllers. I have never considered this before. Though, it's surely expensive to test multiple poses each step, but a single one might make sense.