How to teleport bodies

[JoeJ]

When setting up my ragdoll, i do some change on the pose after body creation so the initial state is already balanced.
But when teleporting the bodies to the new, slightly different state, sometimes i get an assert here:

Code: Select all

void ndBodyDynamic::IntegrateGyroSubstep(const ndVector& timestep)
{
   const ndFloat32 omegaMag2 = m_omega.DotProduct(m_omega).GetScalar();
   const ndFloat32 tol = (ndFloat32(0.0125f) * ndDegreeToRad);
   if (omegaMag2 > (tol * tol))
   {
#ifdef D_USE_FULL_INERTIA
      ndAssert(0);
#else
      const ndFloat32 omegaAngle = ndSqrt(omegaMag2);
      const ndVector omegaAxis(m_omega.Scale(ndFloat32(1.0f) / omegaAngle));
      const ndQuaternion rotationStep(omegaAxis, omegaAngle * timestep.GetScalar());
      m_gyroRotation = m_gyroRotation * rotationStep;
      ndAssert((m_gyroRotation.DotProduct(m_gyroRotation).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-5f)); // <-----------
      
      // calculate new Gyro torque and Gyro acceleration
      const ndMatrix matrix(m_gyroRotation, ndVector::m_wOne);
#endif

      const ndVector localOmega(matrix.UnrotateVector(m_omega));
      const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
      m_gyroTorque = matrix.RotateVector(localGyroTorque);
      m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
   }
   else
   {
      m_gyroAlpha = ndVector::m_zero;
      m_gyroTorque = ndVector::m_zero;
   }
}


So i tried to set everything to zero:

Code: Select all

void TeleportBodiesToPose ()
   {
      for (int i=0; i<NUM_BODIES; i++)
      {
         ndBodyDynamic *body = m_bodies[i]->GetAsBodyDynamic();
         const auto &pose = m_controller->bodyPose[i];

         ndMatrix matrix (ToNdQuat(pose.orn), ToNdPoint(pose.com));
         body->SetMatrix(matrix);   
      
         body->SetOmega(ndVector(0.f, 0.f, 0.f, 0.f));
         body->SetAlpha(ndVector(0.f, 0.f, 0.f, 0.f));
         body->SetVelocity(ndVector(0.f, 0.f, 0.f, 0.f));
         body->SetAccel(ndVector(0.f, 0.f, 0.f, 0.f));
         body->SetForce(ndVector(0.f, 0.f, 0.f, 0.f));
         body->SetTorque(ndVector(0.f, 0.f, 0.f, 0.f));
      }
   }


But did not help, i still get the assert.
Is there something i forgot to set / change?

It's not really a problem, just asking.

[Julio Jerez]

you have a version from when I fix the skew inertia, the function looks like this now

void ndBodyDynamic::IntegrateGyroSubstep(const ndVector& timestep)
{
const ndFloat32 omegaMag2 = m_omega.DotProduct(m_omega).GetScalar();
const ndFloat32 tol = (ndFloat32(0.0125f) * ndDegreeToRad);
if (omegaMag2 > (tol * tol))
{
const ndFloat32 omegaAngle = ndSqrt(omegaMag2);
const ndVector omegaAxis(m_omega.Scale(ndFloat32(1.0f) / omegaAngle));
const ndQuaternion rotationStep(omegaAxis, omegaAngle * timestep.GetScalar());
m_gyroRotation = m_gyroRotation * rotationStep;
ndAssert((m_gyroRotation.DotProduct(m_gyroRotation).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-5f));

// calculate new Gyro torque and Gyro acceleration
const ndMatrix matrix(m_gyroRotation, ndVector::m_wOne);

#ifdef D_USE_FULL_INERTIA
const ndVector localOmega(m_inertiaPrincipalAxis.UnrotateVector(matrix.UnrotateVector(m_omega)));
const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localGyroTorque));
m_gyroAlpha = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localGyroTorque * m_invMass));
#else

no asserts, please try sync.

I am close enough to finish the file load/save, and make a stable release. that past is quite stable now.

[JoeJ]

Hmm, still the same assert after the update.

Posting the new code just to see if there is a difference.
If not, my 'process of doing an update' would be wrong in some way.

Code: Select all

void ndBodyDynamic::IntegrateGyroSubstep(const ndVector& timestep)
{
   const ndFloat32 omegaMag2 = m_omega.DotProduct(m_omega).GetScalar();
   const ndFloat32 tol = (ndFloat32(0.0125f) * ndDegreeToRad);
   if (omegaMag2 > (tol * tol))
   {
      const ndFloat32 omegaAngle = ndSqrt(omegaMag2);
      const ndVector omegaAxis(m_omega.Scale(ndFloat32(1.0f) / omegaAngle));
      const ndQuaternion rotationStep(omegaAxis, omegaAngle * timestep.GetScalar());
      m_gyroRotation = m_gyroRotation * rotationStep;
      ndAssert((m_gyroRotation.DotProduct(m_gyroRotation).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-5f));

      // calculate new Gyro torque and Gyro acceleration
      const ndMatrix matrix(m_gyroRotation, ndVector::m_wOne);

#ifdef D_USE_FULL_INERTIA
      const ndVector localOmega(m_inertiaPrincipalAxis.UnrotateVector(matrix.UnrotateVector(m_omega)));
      const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
      m_gyroTorque = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localGyroTorque));
      m_gyroAlpha = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localGyroTorque * m_invMass));
#else
      const ndVector localOmega(matrix.UnrotateVector(m_omega));
      const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
      m_gyroTorque = matrix.RotateVector(localGyroTorque);
      m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
#endif
   }
   else
   {
      m_gyroAlpha = ndVector::m_zero;
      m_gyroTorque = ndVector::m_zero;
   }
}


Edit: There is a difference, so i have the new version.

Not sure what's wrong. But i have some problems with reseting the ragdoll. It's expected to stand still after the reset, but it makes a little jump instead.
The assert might happen not at the first step (all velocities should be zero), but after the next (maybe because joints cause too large velocities or something like that).

Likely i should add functionality to execute one step of update only after clicking a button...

[JoeJ]

I am close enough to finish the file load/save, and make a stable release.

Don't forget to fix the google cmake issue. Just tried to make sandbox project from the update, but still cmake error here:

Code: Select all

-- Selecting Windows SDK version 10.0.22000.0 to target Windows 10.0.19041.
CMake Error at C:/Program Files/CMake/share/cmake-3.26/Modules/ExternalProject.cmake:2806 (message):
  error: could not find git for clone of googletest-populate


[Julio Jerez]

You need to install git
That a different tool than GitHub.

[JoeJ]

You need to install git

Aha, so i guess i did not click git / cmake checkboxes when installing VS?
But if i do, VS can open cmake projects, and also downloads the missing google stuff?

Explains why i do not see any cmake options in my VS. Will try out next time...

Edit: Installed git as stand alone tool, and now cmake works without issues. : )
Later i'll try to reproduce Clang crashes in the sandbox and report what i'll find...

[Julio Jerez]

not need to involve VS, you just install this: https://git-scm.com/downloads

them CMake will find the depot in Google.

[JoeJ]

cross post, see edit above. thanks!

[JoeJ]

No luck : )
There are 2 unresolved externals build errors about the file stuff you currently work on:

Code: Select all

Build started...
1>------ Build started: Project: ndSandbox, Configuration: Debug x64 ------
1>ndFileFormat_d.lib(ndFileFormatBodyKinematicPlayerCapsule.obj) : error LNK2019: unresolved external symbol "private: void __cdecl ndBodyPlayerCapsule::Init(class ndMatrix const &,float,float,float,float)" (?Init@ndBodyPlayerCapsule@@AEAAXAEBVndMatrix@@MMMM@Z) referenced in function "protected: virtual void __cdecl ndFileFormatBodyKinematicPlayerCapsule::LoadBody(class nd::TiXmlElement const * const,class ndTree<class ndShape *,int,class ndContainersAlloc<class ndShape *> > const &,class ndBody * const)" (?LoadBody@ndFileFormatBodyKinematicPlayerCapsule@@MEAAXQEBVTiXmlElement@nd@@AEBV?$ndTree@PEAVndShape@@HV?$ndContainersAlloc@PEAVndShape@@@@@@QEAVndBody@@@Z)
1>ndFileFormat_d.lib(ndFileFormatShapeCompound.obj) : error LNK2019: unresolved external symbol "protected: __cdecl ndShapeCompound::ndShapeCompound(class ndShapeCompound const &,class ndShapeInstance const * const)" (??0ndShapeCompound@@IEAA@AEBV0@QEBVndShapeInstance@@@Z) referenced in function "public: virtual class ndShape * __cdecl ndFileFormatShapeCompound::LoadShape(class nd::TiXmlElement const * const,class ndTree<class ndShape *,int,class ndContainersAlloc<class ndShape *> > const &)" (?LoadShape@ndFileFormatShapeCompound@@UEAAPEAVndShape@@QEBVTiXmlElement@nd@@AEBV?$ndTree@PEAVndShape@@HV?$ndContainersAlloc@PEAVndShape@@@@@@@Z)
1>C:\dev\newton-dynamics-master\newton-4.00\applications\ndSandbox\Debug\ndSandbox.exe : fatal error LNK1120: 2 unresolved externals
1>Done building project "ndSandbox.vcxproj" -- FAILED.
========== Build: 0 succeeded, 1 failed, 12 up-to-date, 0 skipped ==========
========== Elapsed 00:01.629 ==========


[Julio Jerez]

ahh, sorry, I was building static dlls and I forgot some dlls exports.
It is corrected now, play try again.

Joe, now that I covered enough of the file system,
I am tuning the attention to the zero moment point balancing.

when you sync you will see a unit bike. the closest I can fine to real life is something like this.

Untitled.png (112.35 KiB) Viewed 12912 times

that actually a fictional model, but believe or not that thing is actually dynamically stable.

I select that because it can be done with forward dynamics, so the case of Inver dynamics will not be a source of error. It is also simple enough that we can check that result with the math on paper.
to make even simpler, it is set to move on eth plane, so it is a 2d case.

whet will be different this time is that I will be deriving the equations that I will be using, on that other thread, to make the robot stable.
Them if we success, and only after we succeed, is that I will add complexity.

I set the test case, it does not do anything yet.

[JoeJ]

Haha, that sounds awesome. Monobike Enemy or companion

It reminds me i did not figure out an equation for 'how much do we need to lean forward depending on velocity'. So i did this only with trial and error, but not really understood the reason and dynamics.

I guess you'll have to do this very precisely to make this guy work, so i'll have a look

[Julio Jerez]

Actually Joe you have the right intuition.

Here is a very naive an simple algorithm

We make the leg a hinge, with a strong PD.
This make the whole object a solid.

The say it is going to topple to the front.
Intuition say that if is flex the hinge to the front, it should prevent it from falling.

For that, if we use the IK solver, in the model update, we can just try an arbitrary move in one direction.
And solve using the ik.

Them we sum the total angular momentum.
If the horizontal angular momentum decreases.
The we solve again on the same direction.
If it increases we solve in the opposite direction.

That in calculus is called as bisecting integration.

Using the ik solver we can just do that.

Of course this is very bad, but should work
In fact that's the intuition that is used to derive the formula for the zero moment point criteria.
A zero moment point is the point on a surface where the horizontal torque is zero.
An improvement of the above algorithm, is using the zero moment point to estimate the magnitude and direction of the joints moments. But this is a refinement.

What I am looking is to come up with a set of reliable stepping stone, before moving to the more complex models like biped and quadruped gait driven by ik.