[SOLVED] issue with dcustomHinge

[blackbird_dream]

I modified the FlatLandGame to give an illustration of the issue.
I modelled 4 couples of 2 bodies linked between eachother with 4 customhinge joints.
I just changed the mass of the 2nd body of each couple.
0.01 1.01 2.01 and 3.01
When I hold the 1st body of each couple the hinge joint has always the same angle irrespective of the mass of the child body. The joint stiffness was set with SetAsSpringDamper(...) and same settings for all couples of bodies.

Here the code modified in the FlatlandGame.cpp file (2 methods modified only):

Code: Select all

static void AttachLimbBody (DemoEntityManager* const scene, const dVector& dir, NewtonBody* const parent, float masse)
{
   NewtonWorld* const world = scene->GetNewton();
   dVector size(0.4f, 0.25f, 0.75f, 0.0f);

   NewtonCollision* const collision = CreateConvexCollision(world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
   DemoMesh* const geometry = new DemoMesh("box", scene->GetShaderCache(), collision, "smilli.tga", "logo_php.tga", "frowny.tga");

   dMatrix location;
   NewtonBodyGetMatrix(parent, &location[0][0]);

   location.m_posit += dir.Scale(0.5f);
   location.m_posit.m_y -= 0.5f;

   // make a root body attached to the world
   NewtonBody* const rootBody = CreateSimpleSolid(scene, geometry, masse, location, collision, 0);

   // constrain these object to motion on the plane only
//   location.m_posit -= dir.Scale (0.5f);
   location.m_posit -= dir.Scale(0.5f);

   dCustomHinge* const hinge = new dCustomHinge(location, rootBody, parent);
   //hinge->EnableLimits(true);
   hinge->SetLimits(-45.0f * dDegreeToRad, 45.0f * dDegreeToRad);
   hinge->SetAsSpringDamper(true, 1.f, 500.f, 10.f);


   geometry->Release();
   NewtonDestroyCollision(collision);
}

static void AddRagdollBodies(DemoEntityManager* const scene, NewtonBody* const floor)
{
   NewtonWorld* const world = scene->GetNewton();
   dVector size(0.2f, 1.0f, 0.2f, 0.0f);

   dMatrix location(dGetIdentityMatrix());

   location.m_posit.m_y = 4.0f;

   NewtonCollision* const collision = CreateConvexCollision(world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
   DemoMesh* const geometry = new DemoMesh("box", scene->GetShaderCache(), collision, "smilli.tga", "logo_php.tga", "frowny.tga");

   location.m_posit.m_z = 5.0f;
   float rootmass = 10.f;
   for (int i = 0; i < 4; i++) {
      // make a root body attached to the world
      
      //if (i == 3) { rootmass = 1000.f; }
      
      NewtonBody* const rootBody = CreateSimpleSolid(scene, geometry, rootmass, location, collision, 0);

      // constrain these object to motion on the plane only
      dMatrix matrix;
      NewtonBodyGetMatrix(rootBody, &matrix[0][0]);
   //   new dCustomPlane(matrix.m_posit, matrix.m_front, rootBody);

      // now make some limb body and attach them to the root body
   //   AttachLimbBody (scene, matrix.m_right.Scale ( 1.0f), rootBody);
      AttachLimbBody(scene, matrix.m_right.Scale(-1.0f), rootBody, 1.f * i +0.01f);
   //   AttachLimbBody(scene, matrix.m_right.Scale(-1.0f), rootBody, 0.01f);

      location.m_posit.m_z += 2.5f;
   }

   geometry->Release();
   NewtonDestroyCollision(collision);
}


here is the video illustrating:



here a video capture of the demo when the mass of the parent bodies is set to 0:
https://ibb.co/ckz2qNV

[blackbird_dream]

I forgot to say that there is a ratio >100 between the mass of the 1st child body and the others. So the angle (due to the gravity) should be much lower in the 1st hinge joint.

[Julio Jerez]

I think I explained this a few time, but maybe you do not know, I try again.
The spring damper is is only valid with it some range where some value vanish.
It is possible thre is a bug, those values seem reasonably, but here is the reasoning to the spirit damper function.

Let us say you have two masses connected by a spring and a damper.

The reaction force between the two is given by f = -Kx-cv
This is very unstable to integrate using Eller so what people do is using implicit integration.
This is calculate the acceletion at time t + do and use that as if it was the acceleration at time t.

The result is tha the derivative generate some correction factor for both side of the equation.
Here is the derivation for two masses.

[Julio Jerez]

I wrote in a piece of paper, so that you can see it.
You can check it online by searching implicit integration.
The point is to arrive to the equation that need to be integrated whi is.

(M + dt*c + dt^2*x)*a = Fext-k*x - c*v + dt*c*x

This equation is very similar to what we get when we derive the force between two mass but we relaxed the the main diagonal by adding a regularize and a penalty to the right side.
I do not have time not later I will derive the same equation using the lagrangian and you see thay by analogy we can set the penalty and the mass matrix regularize to the effect of the spring and the damper.

This should be ideal but like every thing it depends on the factor involved, time, mass, k and c.
With this info you will know what to expect.

[blackbird_dream]

Ok I understand.
But the static equilibrium position sould be correct in my opinion, whether the derivative is biased, shouldn't it?
In the case study it isn't.

[Julio Jerez]

ok I pasted the code in the repro and cleaned up few things.

I only placed two bodies, and I fixed the parent to the world with a fix joint.
on the left, the mass is 0.01 and on the right the mass is 1.0
is clearly look as if the are deflecting the same angle and that does not looks right.
if that the problem?

[blackbird_dream]

Absolutely

[Julio Jerez]

ok I look at the function that set the mass matrix regularizer and the penalty of the right side, and I see is not taking the masses of the two bodes into account, this bug has been there for many years and is was because at the time we has the mass ratios problems and most simulation use bodies of more of less equal masses so is was not an issue.
Basically the code there is to make spring that are independent of mass ratios.
at the time I had to do a lot of couching for the users, they use that for vehicle and changing parameters of the vehicle caused tuning spring all over. so is not unreasonably to make springs that are independent of the mass if the bodies involved.
long time ago we solved those issues but those function did not changed.

It is an eassy fix, its just has to scale the regularizer by the inverse of the effective diagonal of the two bodies involved, the funtion is this

Code: Select all

void dgBilateralConstraint::SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 rowStiffness, dgFloat32 spring, dgFloat32 damper)
{
   if (desc.m_timestep > dgFloat32 (0.0f)) {

      dgAssert (m_body1);
      const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
      const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;

      const dgVector& veloc0 = m_body0->m_veloc;
      const dgVector& omega0 = m_body0->m_omega;
      const dgVector& veloc1 = m_body1->m_veloc;
      const dgVector& omega1 = m_body1->m_omega;

      //dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle;
      dgFloat32 relPosit = desc.m_penetration[index];
      dgFloat32 relVeloc = - (veloc0.DotProduct(jacobian0.m_linear) + veloc1.DotProduct(jacobian1.m_linear) + omega0.DotProduct(jacobian0.m_angular) + omega1.DotProduct(jacobian1.m_angular)).GetScalar();

      //at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
      dgFloat32 dt = desc.m_timestep;
      dgFloat32 ks = dgAbs (spring);
      dgFloat32 kd = dgAbs (damper);
      dgFloat32 ksd = dt * ks;
      dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
      dgFloat32 den = dt * kd + dt * ksd;
      dgFloat32 accel = num / (dgFloat32 (1.0f) + den);
      desc.m_diagonalRegularizer[index] = rowStiffness;
      SetMotorAcceleration (index, accel, desc);
   }
}


I do not have time to debug now, I will do it tomorrow morning.

what I will do is the I will add a new function SetSpringDamper
and the existing one will be renamed to Set MassIndepndentSpringDamper.

[blackbird_dream]

Fantastic, you're brilliant.
Great engine

[Julio Jerez]

oh I added these tow functions for setting spring damper row

Code: Select all

void SetMassDependentSpringDamperAcceleration(dgInt32 index, dgContraintDescritor& desc, dgFloat32 spring, dgFloat32 damper);
void SetMassIndependentSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 rowStiffness, dgFloat32 spring, dgFloat32 damper);


as I post before, the formula to calculate the acceleration between two mass connected by a spring damper is given by the equation I posted on the above image.
in the end, it reduces to a first order linear differential if the form dy/dx = a * f(y)
the solutions of these equations are harmonic damped function of the form
y = A * exp(-k0*x) * Cos(k2*x)

this equation most be integrated using a implicit method scheme, which id find the derivation one step in advance and use it as the derivative at time t.

this is equivalent to alter the original equation from

M * x = B

to

1) (M + dM) * x = B + dB

but the is exactly the method the same trick that we use to some linear system of equation when we use the concept of matrix regularize. which is just adding a position small value to to the main diagonal to guarantee that is each gaussian pivot, the norm of the row is ratified.
This is the diagnal^2 > than teh sum of all teh off diagonal elements of the row.
It does no solve the original matrix, but is gives a very close approximation.

with this in mind we can set the regularizer parameter and the penalty on teh right size.

when plugin the spring ad the damper to teh differential equation, the solution was give by this expression

//a = [- ks * (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]

where ks, kd, x, dt are the parameters of teh spring damper system

with this, we can do two things:
1- use the equation are it the penealty on the right side of equation 1 and we make the regularizer an arbitrary value. and since the right side is a accelration that is not affect by the mass, the behavior is a mass independent spring/damper system. This is the method implemented now and is this function.

//dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle;
dgFloat32 relPosit = desc.m_penetration[index];
dgFloat32 relVeloc = - (veloc0.DotProduct(jacobian0.m_linear) + veloc1.DotProduct(jacobian1.m_linear) + omega0.DotProduct(jacobian0.m_angular) + omega1.DotProduct(jacobian1.m_angular)).GetScalar();

Code: Select all

      //at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
      dgFloat32 dt = desc.m_timestep;
      dgFloat32 ks = dgAbs (spring);
      dgFloat32 kd = dgAbs (damper);
      dgFloat32 ksd = dt * ks;
      dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
      dgFloat32 den = dt * kd + dt * ksd;
      dgFloat32 accel = num / (dgFloat32 (1.0f) + den);
      desc.m_diagonalRegularizer[index] = rowStiffness;
      SetMotorAcceleration (index, accel, desc);


2- we can take the denominator and place of the left size and let it be regularizer dM (the fraction of the diagonal for the matrix) and place the numerator of the right size. since diagonal of the matrix is the effective mass matrix of the two bodies, now the denomination modulate the mass matrix, the effect is thet the regularizer make the effect mass larger by a funtion the correlates to the spring mass system. Note that the calculation is not exact. is just correlates to a perfect spring mass system integrated using forward differenced. The implementation now becomes this.

dgFloat32 relPosit = desc.m_penetration[index];
dgFloat32 relVeloc = -(veloc0.DotProduct(jacobian0.m_linear) + veloc1.DotProduct(jacobian1.m_linear) + omega0.DotProduct(jacobian0.m_angular) + omega1.DotProduct(jacobian1.m_angular)).GetScalar();

Code: Select all

      //at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
      dgFloat32 dt = desc.m_timestep;
      dgFloat32 ks = dgAbs(spring);
      dgFloat32 kd = dgAbs(damper);
      dgFloat32 ksd = dt * ks;
      
      dgFloat32 den = dt * kd + dt * ksd;
      dgFloat32 accel = ks * relPosit + kd * relVeloc + ksd * relVeloc;
      desc.m_diagonalRegularizer[index] = dgFloat32(1.0f) / den;
      SetMotorAcceleration(index, accel, desc);


so far I only added it to the Hinge joint, but is eassy to add it to all joint that support spring damper funtionality.

note: I also fixed the way you call set as Spring Damper, you pass 1.0 as the regularizer which is the maximum allowed value. this is why your spring always acted soft, the value is a parameter from 0 to 1.0, 0 been very stiff and 1 soft.

what the new method does use regularizer, instead uses the time step to calculate the regularizer.
It is all commited see if it is what you seek.

[blackbird_dream]

I'm testing.
The problem is that dcustomdoublehinge mixes things and it's all messy

[Julio Jerez]

is added to double hinge

void SetAsSpringDamper1(bool state, dFloat spring, dFloat damper);
void SetMassIndependentSpringDamper1(bool state, dFloat springDamperRelaxation, dFloat spring, dFloat damper);

[blackbird_dream]

ok Thks. I've downloaded the latest build.
There are some weird reactions, but maybe it comes from my model.
I need a couple of hours to check.

[blackbird_dream]

In fact the test doesn't work if we change values.
For instance I used:

Code: Select all

   hinge->SetMassIndependentSpringDamper(true, 0.3f, 10.f, 1.f);


and

Code: Select all

      AttachLimbBody(scene, matrix.m_right.Scale(-1.0f), rootBody, 0.1f * i + 0.001f);


and it gives :
https://ibb.co/jz3Mj2t

[blackbird_dream]

although the ratio of mass is 100 between the 2 bodies.