Development of self balancing biped with inverse Dynamics

[Julio Jerez]

You should not have to set velocity every frame.

Any object should just move at the conveyor velocity as you said. If not that's a very fundamental bug.
I will check it out.

[JoeJ]

Comparing contact generation and placement with everything set back to normal, i see the same distribution to just 2 opposite box corners.
So i guess it's some optimization logic bug. Like parts of the collision system temporally disabled, because Newton wrongly assuming some incorrect state.

It should be easy to reproduce, but let me know otherwise.

[Julio Jerez]

ok, joe I modified demo 1
void ndBasicRigidBody(ndDemoEntityManager* const scene)

instead of making the ground a dynamics body with zero mass, it makes it a dynamics
them it set the velocity to 1 along x.

I see it working, in fact it seems too perfect, if you ask me.
if you sync, maybe can you make the modification to show the problem.

That a good test, I have no play mush with actual kinematic bodies other that making a play capsule.
in theory a kinematic body should be better for a background, than a dynamic with zero inv mass.
It is good for make large structure that moves, stuff like space and vessels ships.

please let me know, if that works.

[JoeJ]

I did the update and built the projects. But i get compiler errors, for example:

Code: Select all

2>ndDemoEntityManager.cpp
2>C:\dev\newton-dynamics-master\newton-4.00\applications\ndSandbox\ndDemoEntityManager.cpp(334,59): error C2259: 'ndBrainGpuContext': cannot instantiate abstract class
2>C:\dev\newton-dynamics-master\newton-4.00\sdk\dBrain\cpuEmulation\ndBrainGpuContext.h(25,7): message : see declaration of 'ndBrainGpuContext'
2>C:\dev\newton-dynamics-master\newton-4.00\applications\ndSandbox\ndDemoEntityManager.cpp(334,59): message : due to following members:
2>C:\dev\newton-dynamics-master\newton-4.00\applications\ndSandbox\ndDemoEntityManager.cpp(334,59): message : 'void ndBrainContext::Less(ndBrainFloatBuffer &,ndBrainFloat)': is abstract


Basically i get this error 6 times:
'ndBrainGpuContext': cannot instantiate abstract class

[Julio Jerez]

It looks like there's something wrong with my Windows system and GitHub integration. Occasionally, some files keep an older timestamp or metadata, and even if I modify them locally, Git doesn’t recognize them as changed, so they don’t show up as modified and can’t be committed.

This has happened before and led me on wild goose chases trying to track down bugs that were actually caused by these inconsistencies. It seems like sometimes the local repo gets corrupted and causes GitHub to get confused.

To fix it, I usually have to:

Back up my local files
Delete the local repository
Re-clone it from GitHub
Use Araxis to manually merge my changes
Then commit everything again

I just went through that process. Please try again.

[JoeJ]

Reminds me on a VS bug which haunts me.
Very rarely code changes are ignored. It compiles but maybe using older files cached somewhere.
So i code and code, but nothing changes, i get crazy and then i remember about the bug.
'Clean Solution' and rebuilding everything fixes it. Maybe that's enough for you as well.

I could reproduce the issues.
But my code has no visual Demo meshes, so you need to switch to 'show solid collision' after the demo starts.
Then you see 2 boxes falling down, moving over the floor, and before they reach the edge of the screen both stop moving and one starts wobbling.

Code: Select all

/* Copyright (c) <2003-2022> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/

#include "ndSandboxStdafx.h"
#include "ndSkyBox.h"
#include "ndDemoMesh.h"
#include "ndDemoCamera.h"
#include "ndPhysicsUtils.h"
#include "ndPhysicsWorld.h"
#include "ndMakeStaticMap.h"
#include "ndDemoEntityNotify.h"
#include "ndDemoEntityManager.h"
#include "ndDemoInstanceEntity.h"



class ContactNotify : public ndContactNotify
{
   public:

   bool disable = 0;

   ContactNotify(ndScene* const scene) : ndContactNotify(scene)
   {
   }

   bool OnAabbOverlap(const ndContact* const contact, ndFloat32 timestep) const
   {
      return (!disable);
   }
};

static ndVector gravity (0.0f, -10.0f, 0.0f, 0.0f);

class BodyNotify : public ndBodyNotify
{
   public:
   BodyNotify()
      :ndBodyNotify(gravity)
   {
      // here we set the application user data that
      // goes with the game engine, for now is just null
      m_applicationUserData = nullptr;
   }

   void OnApplyExternalForce(ndInt32, ndFloat32)
   {
      ndBodyDynamic* const dynamicBody = GetBody()->GetAsBodyDynamic();
      if (dynamicBody)
      {
         ndVector massMatrix(dynamicBody->GetMassMatrix());
         ndVector force(gravity.Scale(massMatrix.m_w));
         dynamicBody->SetForce(force);
         dynamicBody->SetTorque(ndVector::m_zero);
      }
   }

   void OnTransform(ndInt32, const ndMatrix&)
   {
   }

   void* m_applicationUserData;
};




static ndBodyDynamic* BuildBox (ndWorld &world, const ndMatrix& xform, ndFloat32 mass, const ndVector dim)
{
   ndShapeInstance box(new ndShapeBox(dim[0], dim[1], dim[2]));
   ndBodyDynamic* const body = new ndBodyDynamic();

   body->SetNotifyCallback(new BodyNotify);
   body->SetMatrix(xform);
   body->SetCollisionShape(box);
   if (mass>0.f) body->SetMassMatrix(mass, box);

   ndSharedPtr<ndBody> bodyPtr(body);
   world.AddBody(bodyPtr);
   return body;
}

static ndBodyKinematic* BuildKinematicBox (ndWorld &world, const ndMatrix& xform, ndFloat32 mass, const ndVector dim)
{
   assert (mass > 0.f);
   ndShapeInstance box(new ndShapeBox(dim[0], dim[1], dim[2]));
   ndBodyKinematic* const body = new ndBodyKinematic();

   body->SetNotifyCallback(new BodyNotify);
   body->SetMatrix(xform);
   body->SetCollisionShape(box);
   if (mass>0.f) body->SetMassMatrix(mass, box);

   ndSharedPtr<ndBody> bodyPtr(body);
   world.AddBody(bodyPtr);
   return body;
}




void ndBasicRigidBody(ndDemoEntityManager* const scene)
{
   constexpr ndFloat32 groundHeight = 0.f;
   constexpr double PI = 3.1415926535897932384626433832795029;

   ndPhysicsWorld &world = *scene->GetWorld();

   world.SetSubSteps(2);
   world.SetSolverIterations(12);
   world.SetThreadCount(1);
   ContactNotify *contactNotify = new ContactNotify(world.GetScene());
   world.SetContactNotify(contactNotify);
         
   ndVector origin(0.0f, 0.0f, 0.0f, 1.0f);
   ndMatrix xform = ndGetIdentityMatrix();

   ndMatrix groundXF;
   ndBody *groundBody = 0;
   if (1) // flat floor and walls
   {
      ndFloat32 angle = 0.f / 180.f * float(PI);
      ndQuaternion q (ndVector (0.f,0.f,1.f,0.f), angle);
      groundXF = ndCalculateMatrix (q, origin + ndVector(15.f, groundHeight + 4.f, 0.f, 0.f));

      ndFloat32 size = 100;
      groundXF.m_posit = origin + ndVector(0.f, -.5f+groundHeight, 0.f, 0.f);
      //ndBodyDynamic* bodyFloor = BuildBox(world, groundXF, 0.f, ndVector(size * 2.f, 1.0f, size * 2.f, 0.0f));
      ndBodyKinematic* bodyFloor = BuildKinematicBox(world, groundXF, 10000.f, ndVector(size * 2.f, 1.0f, size * 2.f, 0.0f));
      groundBody = (ndBody*)bodyFloor;

      bodyFloor->SetVelocity(ndVector(0.f,0.f,-0.1f,0.f)); // does not work; dynamic box stops moving and starts wobbling after 10 sec (newton bug?)

      ndMatrix xf = groundXF;
      xf.m_posit = origin + xf.RotateVector(ndVector( size, 0.f, 0.f, 0.f)); BuildBox(world, xf, 0.f, ndVector(1.0f, 5.0f, size * 2.f, 0.0f));
      xf.m_posit = origin + xf.RotateVector(ndVector(-size, 0.f, 0.f, 0.f)); BuildBox(world, xf, 0.f, ndVector(1.0f, 5.0f, size * 2.f, 0.0f));
      xf.m_posit = origin + xf.RotateVector(ndVector(0.f, 0.f,  size, 0.f)); BuildBox(world, xf, 0.f, ndVector(size * 2.f, 5.0f, 1.0f, 0.0f));
      xf.m_posit = origin + xf.RotateVector(ndVector(0.f, 0.f, -size, 0.f)); BuildBox(world, xf, 0.f, ndVector(size * 2.f, 5.0f, 1.0f, 0.0f));
   }

   if (1) // dynamic box, stops and starts wobbling
   {
      xform.m_posit = origin + ndVector(7.0f, 10.0f, 0.0f, 0.0f);
      ndBodyDynamic* box = BuildBox (world, xform, 10.f, ndVector (5.f, 0.5f, 1.0f, 0.f));
   }

   if (1) // another dynamic box, stops at the same time
   {
      xform.m_posit = origin + ndVector(0.0f, 7.0f, 0.0f, 0.0f);
      ndBodyDynamic* box = BuildBox (world, xform, 10.f, ndVector (1.f, 0.5f, 1.0f, 0.f));
   }



   ndMatrix matrix(ndGetIdentityMatrix());
   matrix.m_posit.m_x -= 5.0f;
   matrix.m_posit.m_y += 2.0f;
   ndQuaternion rotation(ndVector(0.0f, 1.0f, 0.0f, 0.0f), -30.0f * ndDegreeToRad);
   scene->SetCameraMatrix(rotation, matrix.m_posit);
}




[Julio Jerez]

alright.
I adapted the demo to use visual meshes.

I do see w jitter of the boxes, as they slide over the box,
That seem a problem. I will check that.

the boxes seem to move straight over the box floor.
Tell me if my changes reflect the problems you are having.

edit:
wow, I think I reproduced if I change the box floor to a flat plane.
I assume that' the bug you as seem, and yes, it is really bad.

I never seeing it so bad.
The only good point is that it wobbles even of there is not velocity assigned to the bug.
also, the bug is still there if the floor box is dynamics body.
that rules out the contact joint and points the bug to the contact solver.

the box is relatively large 5 meter in length.
It seems the optimization heuristic does assumption that fail with large objects.
but who knows, I will debug it.

[Julio Jerez]

ok I start debugging it, and I found the first problem.
In the code bellow

Code: Select all

      //ndFloat32 size = 100;
      //ndBodyDynamic* bodyFloor = BuildBox(world, groundXF, 0.f, ndVector(size * 2.f, 1.0f, size * 2.f, 0.0f));
      //ndBodyKinematic* bodyFloor = BuildKinematicBox(world, groundXF, 10000.f, ndVector(size * 2.f, 1.0f, size * 2.f, 0.0f));
      //ndSharedPtr<ndBody> bodyFloor (BuildFloorBox(scene, groundXF, true));
      ndSharedPtr<ndBody> bodyFloor(BuildFlatPlane(scene, true, true));

      groundBody = *bodyFloor;
      // does not work; dynamic box stops moving and starts wobbling after 10 sec (newton bug?)
      bodyFloor->SetVelocity(ndVector(0.f, 0.f, -0.1f, 0.f));
      bodyFloor->SetVelocity(ndVector(0.f, 0.f, 0.0f, 0.f));

      //ndMatrix xf = groundXF;
      //xf.m_posit = origin + xf.RotateVector(ndVector(size, 0.f, 0.f, 0.f));
      //BuildBox(world, xf, 0.f, ndVector(1.0f, 5.0f, size * 2.f, 0.0f));
      //
      //xf.m_posit = origin + xf.RotateVector(ndVector(-size, 0.f, 0.f, 0.f));
      //BuildBox(world, xf, 0.f, ndVector(1.0f, 5.0f, size * 2.f, 0.0f));
      //
      //xf.m_posit = origin + xf.RotateVector(ndVector(0.f, 0.f, size, 0.f));
      //BuildBox(world, xf, 0.f, ndVector(size * 2.f, 5.0f, 1.0f, 0.0f));
      //
      //xf.m_posit = origin + xf.RotateVector(ndVector(0.f, 0.f, -size, 0.f));
      //BuildBox(world, xf, 0.f, ndVector(size * 2.f, 5.0f, 1.0f, 0.0f));

That place for more floor boxes, the create a large overlapping area.
but those ha zero velocity, I thought that was the cause of the problem, but
after commenting it out, the wobbling is still there.
The engine should be able to deal with that that, but for this test,
that's not necessary to recreate the bug.
I will comment it out for the moment, to make it easier to debug.

I can discuss that idea later.

[Julio Jerez]

wow, joe your insight was spot on.

The code that I added to prune contact has a rare bug.

when the covariance matrix has a zero entry in the main diagonal, the eigen values come out wrong.
and it may mick a wrong average plane to select contacts.

It is not fix yet, but that seem to be the problem.
I not sure how to figure that out, but I am very impressed.

It is here
ndInt32 ndContactSolver::ConvexContactsDiscrete()

by line 2520, the contacts always have the same height, that produces a covariance matrix with a zero entry on that diagonal.

after that it is all a manipulation of the matrix to see with direction is the best plane for contacts,
and the heuristic code fail in that case.

I do have a chek fo that.

Code: Select all

   for (ndInt32 i = 0; i < 3; ++i)
   {
      if (ndAbs(covariance[i][i]) < ndFloat32(1.0e-8f))
      {
         for (ndInt32 j = 0; j < 3; ++j)
         {
            covariance[i][j] = ndFloat32(0.0f);
            covariance[j][i] = ndFloat32(0.0f);
         }
         covariance[i][i] = ndFloat32(1.0e-8f);
      }
   }


It seems that in present of large value, and diagonal of 1.0e-8 is maybe too small
My guess is that the best value is some scale of the smaller non zero value of the main diagonal
I am working on fixing it.

[JoeJ]

when the covariance matrix has a zero entry in the main diagonal, the eigen values come out wrong.
and it may mick a wrong average plane to select contacts.

Oh, interestingly i worked on a very similar problem just recently. I made the ragdoll walk on a sloped plane, and for that i wanted to calculate the average ground plane from 'contact points' (in my case coming from ray casts below the feet).

This math is still pretty new to me. I've learned it from the fluid simulator.
Like you i sum up a covariance matrix, but then i do 3x3 singular value decomposition, basically fitting an ellipsoid to the points, and it's shortest axis gives the plane.
I wonder if the results of both approaches are equivalent. This stuff is really cool math, but i had to get almost 50 years old to learn it.

However, in case the points form a line or a single point, i was thinking i'll have to use contact surface normals to deal with such failure cases.

[Julio Jerez]

Eigenvalue decomposition is a common operation used in many linear algebra problems.
Singular Value Decomposition (SVD) is the more general approach, applicable to all types of matrices, including singular or rank-deficient ones, but it's significantly more computationally expensive.

For certain types of matrices, such as square, symmetric, and positive definite ones, there are more efficient methods to compute eigenvectors. Jacobi iteration, for example, is a specialized form of SVD optimized for positive semi-definite (PSD) matrices.

Covariance matrices happen to be symmetric and positive definite, which makes them well-suited for this method.

In 99.9999% of cases, the current implementation works fine. However, for matrices with very large and smaller values, the technique I used to enforce PSD can lose precision due to floating-point limitations adding very small value to large numbers.
This can lead to one axis of the resulting ellipsoid having near-zero length, or worse not converge and terminate with erroneous values.

[Julio Jerez]

Ok Joe try again, see of that works for you now.

With the exception of adding the visualization. I added all you code.
including the part that I was dubious about, but to my surplice it works.
The part is this

Code: Select all

#if 1
      // Joe, this works, but maybe you should check what is the
      // purpose of having so many overlaping background objects
      ndFloat32 size = 100;
      ndMatrix xf = groundXF;
      xf.m_posit = origin + xf.RotateVector(ndVector(size, 0.f, 0.f, 0.f));
      BuildBox(world, xf, 0.f, ndVector(1.0f, 5.0f, size * 2.f, 0.0f));


I put a comment there because I would be surprised if it does not behave as you expect since it will most likely get contacts on the same position with different velocity. and the optimization may prune some but not others, leaving a contact set with different velocities every frame.

anyway, I test it and again, it you ask me It even seem too perfect.

[Julio Jerez]

If you synced, please try again.
I am trying a simple heuristic to discriminate contacts.

It needs to be teste because, it is difficult to predict if is better or worse only based on intuitions.

The reason why this work, is because joint is about
reducing degree of freedom, and eigne vector is also about reducing degree of freedom.

there is a technique called principal component analysis, that given a signal with many components, you can reduce it to the eigen vectors. then you can represent the signal with the eigen vector that has the larger components.
The reduced space is called a latent space. which is just the set of eigen vectors with the dominate eigen values.
That's what the prune function does for contacts.
But it is harder to get it right than just explaining it.

the thing is that at least for me, it is by far the contact filter that produces the best result.
In the pass I try different methods, by this one seems the best so far.

[JoeJ]

Yay, no more wobbling.

But the bodies still stop moving at the same time.
They also sink in into the floor quite a bit at this point.
If i pick them up and let them fall again, they still sink in and do not not move.
So the velocity seems ignored, and the contacts become very soft.

I've noticed this only by pasting in my code snippet again.
With your demo they keep moving correctly, but not with mine.

So i have removed the 4 overlapping bodies forming a fence around my ground box, and then it all works as expected.

We had talked about issues from overlapping bodies before, but i had not thought about that.

So i can continue with the conveyor belt experiment...
But likely it won't help me on my main problem.
The fastest walking speed of my ragdoll is currently 0.5 meter per second.
But humans can walk 1.3 mps.
My ragdoll is too cautious, but it's difficult to overcome this.

I also suspect that walking is not just 'falling forwards', but there also is a 'push the ground backwards' force applied from the stance foot when walking fast.
Sadly, walking is not as easy is i thought it would be...

[Julio Jerez]

So the velocity seems ignored, and the contacts become very soft.

I've noticed this only by pasting in my code snippet again.
With your demo they keep moving correctly, but not with mine.

So i have removed the 4 overlapping bodies forming a fence around my ground box, and then it all works as expected.
.

Oh are you saying the other for bodies are fences, not overlapping extra floor?
What happen when the other bodies hit the fences?
Shouldn't they stops moving.

Overlapping shouldn't be a problem, that's what the filter is for.

But I am not sure what should happen when the body hit a wall. I suppose it should stop.

But not sure how the force solver will interpret the joint with different contacts.

I will make the extra bodies visible, to see what happens at that point.