How to override contact normal force?

[JoeJ]

I want to mess up contact forces, but gently

Inspired from this code from ndContactMaterial,

Code: Select all

void OverrideFriction0Accel(ndFloat32 accel)
   {
      m_dir0_Force.m_force = accel;
      m_material.m_flags = m_material.m_flags | m_override0Accel;
   }


I have tried this:

Code: Select all

            ndBody *thisBody = *m_bodies[body.bodyIndex];
            const ndBodyKinematic::ndContactMap& contactMap = thisBody->GetAsBodyDynamic()->GetContactMap();
            ndBodyKinematic::ndContactMap::Iterator it(contactMap);
            for (it.Begin(); it; it++)
            {
               ndContact* const contact = *it;
               if (contact->IsActive())
               {
                  ndContactPointList& contactPoints = contact->GetContactPoints();
                  
                  const ndBodyKinematic* otherBody = contact->GetBody1(); // assumption: Body1 is always the other body
                  if (otherBody == thisBody->GetAsBodyKinematic())
                     otherBody = contact->GetBody0();
                  assert(otherBody != thisBody->GetAsBodyKinematic());

                  for (ndContactPointList::ndNode* contactNode = contactPoints.GetFirst(); contactNode; contactNode = contactNode->GetNext())
                  {
                     ndContactMaterial& contactPoint = contactNode->GetInfo();
                     contactPoint.m_normal_Force.m_force = 2000.f; // no effect
                     contactPoint.m_material.m_flags = contactPoint.m_material.m_flags | m_overrideNormalAccel;



I have tried to call this from both ndModel::Update() and PostUpdate().
But nothing happens. The simulation ignores my changes, only my contact visualization shows them.

Is there something i do wrong, or can't this work at all?

[JoeJ]

I have tried some more:

Code: Select all

                     ndContactMaterial& contactPoint = contactNode->GetInfo();
                     if (0)
                     {
                        contactPoint.m_normal_Force.m_force = -2000.f;
                        contactPoint.m_material.m_flags = contactPoint.m_material.m_flags | m_overrideNormalAccel; // no effect
                     }
                     if (1)
                     {
                        contactPoint.m_dir0_Force.m_force = 2000.f;
                        //contactPoint.m_material.m_flags = contactPoint.m_material.m_flags | m_override0Friction; // no effect
                        contactPoint.m_material.m_flags = contactPoint.m_material.m_flags | m_override0Accel; // has effect
                     }



I have found that overriding 'accel' has an effect as expected, but 'friction' has not.
However, i realize i'm confused about involved terms of 'force, acceleration, and friction'.
So maybe i should better describe what i want to do.

Currently i use a weighted average of contact points from ragdoll feet, and the weight is contactPoint.m_normal_Force.m_force.
This gives me the measured center of pressure. I visualize it to compare it with the output of my balance controller.
The results are close, but not the same. Simulation lags behind temporally and is more noisy.

Now my idea is to modify contact forces slightly to correct the simulation error.
This may be a bad idea. Or it might actually work, giving me much higher accuracy with no downsides.
(The methods i currently use to deal with simulation error are quite ugly.)

[JoeJ]

Bump on this. I have verified all my math again and again. I also came up with methods to predict and compensate the error, but this only works for simplified models under ideal conditions.
I need more control over contacts, otherwise i can simulate an old man but not Bruce Lee. Precise but indirect control over the COP using joints seems not possible, and i need some direct way.

The primary problem is: Newton takes to long to move the COP to the right place. The COP is too close to the COM. After some frames it becomes better, but til then we already get invalidation of contact points and the process restarts with a big error on the COP again. In practice the COP jumps around constantly and the whole situation is discontinuous and unstable all the time.

The problem is amplified by how balancing works: We move the COP from the heel to the toe quickly to generate fast motion. That's a discontinuity generated by the controller. But if we try to smooth it out, we no longer can do the fast motion we need.
Btw, i doubt machine learning could learn a way to predict and compensate such chaotic error. I've tried this for many years and still do, but it simply does not work.

So we may need a specific contact solution for dynamic characters no matter what and who.

I see 3 options to address this:

1. Changing or setting contact normal force as asked above.

2. Contacts generated by the user. I would use just a single contact per foot and place it at the desired COP. Then Newton would have no other chance than calculating the right ground reaction force, but using just a single contact point may have side effects.

Options not requiring changes to the engine:

3. Disable contact detection and use a joint instead. I have done something like this years ago when i experimented with gradually scaling bodies during the simulation. It has worked pretty well, so maybe i should try again. I don't know much about friction though, but it may work well enough to move on.

4. Use external force to compensate error. This should work perfectly, but we loose purpose to use a physics engine at all. I could just do procedural animation at this point.

Any advice on preferred options 1 and 2 appreciated. I do not really know where in the engine i could implement related hacks.

[JoeJ]

Ok, i have tried to disable default contacts and use a single custom joint instead.
I have tried it only on a simple two bodies pendulum, but results are at least promising. It solves all my problems at least for now.

But i could use some help on the joint. I tried to copy code from dgContact, but it does not work. The joint does nothing, although stepping through with debugger shows expected values.
Something must be wrong, like enabling something or setting a flag.

So i have added a hack to model a very simple joint as usual, which works, but i would like to make it behave like default contacts do.

Code: Select all

class ContactJoint : public ndJointBilateralConstraint
{
public:

   bool enabled = true;
   Vec3 pos0, pos1;
   Vec3 normal, friction0, friction1;
   // measure...
   //Vec3 force;

   
   ContactJoint (ndBodyKinematic* body0, ndBodyKinematic* body1)
      : ndJointBilateralConstraint(4, body0, body1, dGetIdentityMatrix())
   {
      normal = Vec3 (1,0,0);
      friction0 = Vec3 (0,1,0);
      friction1 = Vec3 (0,0,1);

      //SetSolverModel(m_jointIterativeSoft);
      SetSolverModel(m_jointkinematicOpenLoop);
      //SetSolverModel(m_jointkinematicCloseLoop);   
      //SetSolverModel(m_jointkinematicAttachment);   
   }

   void JacobianDerivative (ndConstraintDescritor& desc)
   {
      if (!enabled) return;

      //m_body0->SetSleepState(false);
      //m_body1->SetSleepState(false);

      constexpr ndFloat32 D_REST_RELATIVE_VELOCITY      (1.0e-3f);
      constexpr ndFloat32 D_MAX_DYNAMIC_FRICTION_SPEED   (0.3f);
      constexpr ndFloat32 D_MAX_PENETRATION_STIFFNESS      (50.0f);
      constexpr ndFloat32 D_DIAGONAL_REGULARIZER         (1.0e-3f);

      ndContactMaterial contact;
      ndInt32 normalIndex = 0, frictionIndex = 1;

      //contact.m_point = ToNdVector((pos0 + pos1) * .5f);
      contact.m_point = ToNdPoint(pos0);
      contact.m_normal = ToNdVector(normal);
      contact.m_dir0 = ToNdVector(friction0);
      contact.m_dir1 = ToNdVector(friction1);
      contact.m_penetration = contact.m_normal.DotProduct(ToNdVector(pos1) - contact.m_point).GetScalar();

      contact.m_material.m_restitution = 0.f;

      contact.m_body0 = m_body0;
      contact.m_body1 = m_body1;
      contact.m_shapeId0 = 0;
      contact.m_shapeId1 = 0;
      contact.m_shapeInstance0 = 0;
      contact.m_shapeInstance1 = 0;
      
      if (1) // temporary hack because code below does not yet work
      {
         const float maxFriction = 1000.f;
         const float maxTwist = 5.f;

         AddLinearRowJacobian(desc, ToNdVector(pos0), ToNdVector(pos1), contact.m_normal);
         SetLowerFriction (desc, 0);
         //SetHighFriction (desc, 0);
         AddLinearRowJacobian(desc, ToNdVector(pos0), ToNdVector(pos1), contact.m_dir0);
         SetLowerFriction (desc, -maxFriction);
         SetHighFriction (desc, maxFriction);
         AddLinearRowJacobian(desc, ToNdVector(pos0), ToNdVector(pos1), contact.m_dir1);
         SetLowerFriction (desc, -maxFriction);
         SetHighFriction (desc, maxFriction);

         // add some angular friction to limit twist around gravity dir
         const ndVector omega0(m_body0->GetOmega());
         const ndVector omega1(m_body1->GetOmega());
         ndVector relAcc = (omega1 - omega0) * desc.m_invTimestep;
         dVector gravityDir (0.f, -1.f, 0.f, 0.f);
         ndFloat32 gRelAcc = .3f * relAcc.DotProduct(gravityDir).GetScalar();
         AddAngularRowJacobian (desc, gravityDir, 0.f);
         SetMotorAcceleration (desc, gRelAcc);
         SetLowerFriction (desc, -maxTwist);
         SetHighFriction (desc, maxTwist);
         return;
      }



      auto CalculatePointDerivative = [&](ndInt32 index, ndConstraintDescritor& desc, const ndVector& dir, const ndPointParam& param)
      {
         ndAssert(m_body0);
         ndAssert(m_body1);

         ndJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
         ndJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
         jacobian0.m_linear = dir;
         jacobian1.m_linear = dir * ndVector::m_negOne;

         jacobian0.m_angular = param.m_r0.CrossProduct(dir);
         jacobian1.m_angular = dir.CrossProduct(param.m_r1);

         ndAssert(jacobian0.m_linear.m_w == ndFloat32(0.0f));
         ndAssert(jacobian0.m_angular.m_w == ndFloat32(0.0f));
         ndAssert(jacobian1.m_linear.m_w == ndFloat32(0.0f));
         ndAssert(jacobian1.m_angular.m_w == ndFloat32(0.0f));
      };




      ndPointParam pointData;
      InitPointParam(pointData, contact.m_point, contact.m_point);
      CalculatePointDerivative(normalIndex, desc, contact.m_normal, pointData);

      const ndVector omega0(m_body0->GetOmega());
      const ndVector omega1(m_body1->GetOmega());
      const ndVector veloc0(m_body0->GetVelocity());
      const ndVector veloc1(m_body1->GetVelocity());
      const ndVector gyroAlpha0(m_body0->GetGyroAlpha());
      const ndVector gyroAlpha1(m_body1->GetGyroAlpha());

      ndAssert(contact.m_normal.m_w == ndFloat32(0.0f));
      const ndJacobian& normalJacobian0 = desc.m_jacobian[normalIndex].m_jacobianM0;
      const ndJacobian& normalJacobian1 = desc.m_jacobian[normalIndex].m_jacobianM1;
      const ndFloat32 restitutionCoefficient = contact.m_material.m_restitution;

      ndFloat32 relSpeed = -(normalJacobian0.m_linear * veloc0 + normalJacobian0.m_angular * omega0 + normalJacobian1.m_linear * veloc1 + normalJacobian1.m_angular * omega1).AddHorizontal().GetScalar();
      ndFloat32 penetration = ndClamp(contact.m_penetration - D_RESTING_CONTACT_PENETRATION, ndFloat32(0.0f), ndFloat32(0.5f));
      desc.m_flags[normalIndex] = ndInt32(contact.m_material.m_flags & m_isSoftContact);
      desc.m_jointSpeed[normalIndex] = ndFloat32 (0.0f);
      desc.m_penetration[normalIndex] = penetration;
      desc.m_restitution[normalIndex] = restitutionCoefficient;
      desc.m_forceBounds[normalIndex].m_low = ndFloat32(0.0f);
      desc.m_forceBounds[normalIndex].m_normalIndex = D_INDEPENDENT_ROW;
      desc.m_forceBounds[normalIndex].m_jointForce = (ndForceImpactPair*)&contact.m_normal_Force;

      const ndFloat32 restitutionVelocity = (relSpeed > D_REST_RELATIVE_VELOCITY) ? relSpeed * restitutionCoefficient : ndFloat32(0.0f);
      const ndFloat32 penetrationStiffness = D_MAX_PENETRATION_STIFFNESS * contact.m_material.m_softness;
      const ndFloat32 penetrationVeloc = penetration * penetrationStiffness;
      ndAssert(ndAbs(penetrationVeloc - D_MAX_PENETRATION_STIFFNESS * contact.m_material.m_softness * penetration) < ndFloat32(1.0e-6f));
      desc.m_penetrationStiffness[normalIndex] = penetrationStiffness;
      relSpeed += ndMax(restitutionVelocity, penetrationVeloc);

      const bool isHardContact = !(contact.m_material.m_flags & m_isSoftContact);
      desc.m_diagonalRegularizer[normalIndex] = isHardContact ? D_DIAGONAL_REGULARIZER : ndMax(D_DIAGONAL_REGULARIZER, contact.m_material.m_skinMargin);
      const ndFloat32 relGyro = (normalJacobian0.m_angular * gyroAlpha0 + normalJacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar();

      desc.m_jointAccel[normalIndex] = relGyro + relSpeed * desc.m_timestep;
      if (contact.m_material.m_flags & m_overrideNormalAccel)
      {
         ndAssert(0);
         desc.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force;
      }
      //return;

      // first dir friction force
      //if (contact.m_material.m_flags & m_friction0Enable)
      {
         ndInt32 jacobIndex = frictionIndex;
         frictionIndex += 1;
         ndAssert(contact.m_dir0.m_w == ndFloat32(0.0f));
         CalculatePointDerivative(jacobIndex, desc, contact.m_dir0, pointData);

         const ndJacobian &jacobian0 = desc.m_jacobian[jacobIndex].m_jacobianM0;
         const ndJacobian &jacobian1 = desc.m_jacobian[jacobIndex].m_jacobianM1;
         ndFloat32 relVelocErr = -(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1).AddHorizontal().GetScalar();

         desc.m_flags[jacobIndex] = 0;
         desc.m_forceBounds[jacobIndex].m_normalIndex = (contact.m_material.m_flags & m_override0Friction) ? D_INDEPENDENT_ROW : normalIndex;
         desc.m_diagonalRegularizer[jacobIndex] = D_DIAGONAL_REGULARIZER;

         desc.m_restitution[jacobIndex] = ndFloat32(0.0f);
         desc.m_penetration[jacobIndex] = ndFloat32(0.0f);
         desc.m_jointSpeed[normalIndex] = ndFloat32(0.0f);

         desc.m_penetrationStiffness[jacobIndex] = ndFloat32(0.0f);
         if (contact.m_material.m_flags & m_override0Accel)
         {
            // note: using restitution been negative to indicate that the acceleration was override
            desc.m_restitution[jacobIndex] = ndFloat32(-1.0f);
            desc.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force;
         }
         else
         {
            const ndFloat32 relFrictionGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar();
            desc.m_restitution[jacobIndex] = ndFloat32(0.0f);
            //desc.m_jointAccel[jacobIndex] = relFrictionGyro + relVelocErr * desc.m_timestep;
            desc.m_jointAccel[jacobIndex] = relFrictionGyro + relVelocErr * desc.m_invTimestep;
         }
         if (ndAbs(relVelocErr) > D_MAX_DYNAMIC_FRICTION_SPEED)
         {
            desc.m_forceBounds[jacobIndex].m_low = -contact.m_material.m_dynamicFriction0;
            desc.m_forceBounds[jacobIndex].m_upper = contact.m_material.m_dynamicFriction0;
         }
         else
         {
            desc.m_forceBounds[jacobIndex].m_low = -contact.m_material.m_staticFriction0;
            desc.m_forceBounds[jacobIndex].m_upper = contact.m_material.m_staticFriction0;
         }
         desc.m_forceBounds[jacobIndex].m_jointForce = (ndForceImpactPair*)&contact.m_dir0_Force;
      }

      //if (contact.m_material.m_flags & m_friction1Enable)
      {
         ndInt32 jacobIndex = frictionIndex;
         frictionIndex += 1;
         ndAssert(contact.m_dir1.m_w == ndFloat32(0.0f));
         CalculatePointDerivative(jacobIndex, desc, contact.m_dir1, pointData);

         const ndJacobian &jacobian0 = desc.m_jacobian[jacobIndex].m_jacobianM0;
         const ndJacobian &jacobian1 = desc.m_jacobian[jacobIndex].m_jacobianM1;
         ndFloat32 relVelocErr = -(jacobian0.m_linear * veloc0 + jacobian0.m_angular * omega0 + jacobian1.m_linear * veloc1 + jacobian1.m_angular * omega1).AddHorizontal().GetScalar();

         desc.m_flags[jacobIndex] = 0;
         desc.m_forceBounds[jacobIndex].m_normalIndex = (contact.m_material.m_flags & m_override1Friction) ? D_INDEPENDENT_ROW : normalIndex;
         desc.m_diagonalRegularizer[jacobIndex] = D_DIAGONAL_REGULARIZER;

         desc.m_restitution[jacobIndex] = ndFloat32(0.0f);
         desc.m_penetration[jacobIndex] = ndFloat32(0.0f);
         desc.m_jointSpeed[normalIndex] = ndFloat32(0.0f);
         desc.m_penetrationStiffness[jacobIndex] = ndFloat32(0.0f);
         if (contact.m_material.m_flags & m_override1Accel)
         {
            // note: using restitution been negative to indicate that the acceleration was override
            desc.m_restitution[jacobIndex] = ndFloat32(-1.0f);
            desc.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force;
         }
         else
         {
            const ndFloat32 relFrictionGyro = (jacobian0.m_angular * gyroAlpha0 + jacobian1.m_angular * gyroAlpha1).AddHorizontal().GetScalar();
            desc.m_restitution[jacobIndex] = ndFloat32(0.0f);
            //desc.m_jointAccel[jacobIndex] = relFrictionGyro + relVelocErr * desc.m_timestep;
            desc.m_jointAccel[jacobIndex] = relFrictionGyro + relVelocErr * desc.m_invTimestep;
         }
         if (ndAbs(relVelocErr) > D_MAX_DYNAMIC_FRICTION_SPEED)
         {
            desc.m_forceBounds[jacobIndex].m_low = -contact.m_material.m_dynamicFriction1;
            desc.m_forceBounds[jacobIndex].m_upper = contact.m_material.m_dynamicFriction1;
         }
         else
         {
            desc.m_forceBounds[jacobIndex].m_low = -contact.m_material.m_staticFriction1;
            desc.m_forceBounds[jacobIndex].m_upper = contact.m_material.m_staticFriction1;
         }
         desc.m_forceBounds[jacobIndex].m_jointForce = (ndForceImpactPair*)&contact.m_dir1_Force;
      }

      desc.m_rowsCount = 3;

   }
};