search

NVIDIA-Omniverse / PhysX

NVIDIA PhysX SDK
BSD 3-Clause "New" or "Revised" License
2.54k stars 361 forks source link

Static contact forces are not correct on inclined plane #276

Closed cga-cmu closed 4 months ago

cga-cmu commented 5 months ago

Library and Version

PhysX-105.1-blast-5.0.4

Operating System

Ubuntu 22.04

Steps to Trigger Behavior

  1. Put block on slope and collect contact forces

Code Snippet to Reproduce Behavior

/*******************************************************************
Test physx contact forces vs. acceleration, Does F=ma?
*******************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <iostream>
#include <vector>
#include "PxPhysicsAPI.h"
#include "snippetcommon/SnippetPrint.h"
#include "snippetcommon/SnippetPVD.h"
#include "snippetutils/SnippetUtils.h"
#include "snippetrender/SnippetRender.h"
#include "snippetrender/SnippetCamera.h"

/******************************************************************/

using namespace std;
using namespace physx;

/******************************************************************/
// Defines

#define N_XYZ 3
#define XX 0
#define YY 1
#define ZZ 2

// physx has multiple contacts
#define MAX_N_CONTACTS 4

// Typically we use a time step of 1/60 of a second (60Hz) and 10 iterations.
// 8/3 is also good. This provides a high quality simulation in most games
// float timeStep = 1.0f / 60.0f;
#define TIME_STEP (1.0f/60.0f)

/******************************************************************/
/******************************************************************/
// Globals

// Physx stuff
static PxDefaultAllocator gAllocator;
static PxDefaultErrorCallback gErrorCallback;
static PxFoundation* gFoundation = NULL;
static PxPhysics* gPhysics = NULL;
static PxDefaultCpuDispatcher* gDispatcher = NULL;
static PxScene* gScene = NULL;
static PxMaterial* gMaterial = NULL;
static PxPvd* gPvd = NULL;

std::vector<PxVec3> gContactPositions;
std::vector<PxVec3> gContactNormals;
std::vector<PxVec3> gContactImpulses;
std::vector<PxReal> gContactSeparations;

PxRigidDynamic *box_body;
PxRigidStatic *ground_plane;

/****************************************/
// My stuff

PxReal box_half_length = 0.5;

FILE *output_stream; // The stream to log stuff to

float the_time = 0.0f;
PxTransform box_position;
PxVec3 box_velocity;
float box_acceleration[ N_XYZ ];
float box_angle;
double integrated_box_angle = 0.0;
PxVec3 box_angular_velocity;
float box_angular_acceleration;

float last_box_velocity[ N_XYZ ];
float last_box_angular_velocity;

int n_contacts;
float location[ N_XYZ ][ MAX_N_CONTACTS ];
float normal[ N_XYZ ][ MAX_N_CONTACTS ];
float force[ N_XYZ ][ MAX_N_CONTACTS ];
float total_force[ N_XYZ ];

/******************************************************************/
/******************************************************************/
/******************************************************************/

class ContactReportCallback: public PxSimulationEventCallback
{

  // These don't do anything
  void onConstraintBreak(PxConstraintInfo* constraints, PxU32 count)
  { PX_UNUSED(constraints); PX_UNUSED(count); }

  void onWake(PxActor** actors, PxU32 count)
  { PX_UNUSED(actors); PX_UNUSED(count); }

  void onSleep(PxActor** actors, PxU32 count)
  { PX_UNUSED(actors); PX_UNUSED(count); }

  void onTrigger(PxTriggerPair* pairs, PxU32 count)
  { PX_UNUSED(pairs); PX_UNUSED(count); }

  void onAdvance( const PxRigidBody*const*, const PxTransform*, const PxU32 )
  {}

  // This collects the contacts and impulses
  void onContact( const PxContactPairHeader& pairHeader,
          const PxContactPair* pairs,
          PxU32 nbPairs ) 
  {
    std::vector<PxContactPairPoint> contactPoints;

    for( PxU32 i=0; i < nbPairs; i++ )
      {
    PxU32 contactCount = pairs[i].contactCount;
    if( contactCount )
      {
        contactPoints.resize(contactCount);
        pairs[i].extractContacts( &contactPoints[0], contactCount );
        for( PxU32 j = 0; j < contactCount; j++ )
          {
        // world space location
        gContactPositions.push_back( contactPoints[j].position );
        gContactNormals.push_back( contactPoints[j].normal );
        // The impulse applied at the contact point, in world space.
        // Divide by the simulation time step to get a force value.
        gContactImpulses.push_back( contactPoints[j].impulse );
        gContactSeparations.push_back( contactPoints[j].separation );
          }
      }
      }
  }
};

/******************************************************************/

ContactReportCallback gContactReportCallback;

/******************************************************************/
// Decide what contacts get reported

static PxFilterFlags
contactReportFilterShader( PxFilterObjectAttributes attributes0,
               PxFilterData filterData0,
               PxFilterObjectAttributes attributes1,
               PxFilterData filterData1,
               PxPairFlags& pairFlags,
               const void* constantBlock,
               PxU32 constantBlockSize )
{
  PX_UNUSED(attributes0);
  PX_UNUSED(attributes1);
  PX_UNUSED(filterData0);
  PX_UNUSED(filterData1);
  PX_UNUSED(constantBlockSize);
  PX_UNUSED(constantBlock);

  // all initial and persisting reports for everything, with per-point data
  pairFlags = PxPairFlag::eSOLVE_CONTACT
    | PxPairFlag::eDETECT_DISCRETE_CONTACT
    | PxPairFlag::eNOTIFY_TOUCH_FOUND 
    | PxPairFlag::eNOTIFY_TOUCH_PERSISTS
    | PxPairFlag::eNOTIFY_CONTACT_POINTS;
  return PxFilterFlag::eDEFAULT;
}

/******************************************************************/
/******************************************************************/

void process_contacts()
{
  n_contacts = 0;
  for( PxU32 i = 0; i < gContactPositions.size(); i++ )
    {
      if ( n_contacts < MAX_N_CONTACTS )
    {
      for ( int j = 0; j < N_XYZ; j++ )
        {
          location[ j ][ n_contacts ] = gContactPositions[ i ][ j ];
          normal[ j ][ n_contacts ] = gContactNormals[ i ][ j ];
          force[ j ][ n_contacts ] = gContactImpulses[ i ][ j ]/TIME_STEP;
        }
      n_contacts++;
    }
      else
    {
      fprintf( stderr, "Too many contacts\n" );
      exit( -1 );
    }
    }

  for ( int i = 0; i < N_XYZ; i++ )
    total_force[ i ] = 0;

  if ( n_contacts == 0 )
    {
      printf( "no contacts\n" );
      return;
    }

  if ( n_contacts == 1 )
    {
      for ( int i = 0; i < N_XYZ; i++ )
    total_force[ i ] += force[ i ][ 0 ];
      if ( fabsf( force[ XX ][ 0 ] ) > 1e-6 
       || fabsf( force[ YY ][ 0 ] ) > 1e-6 )
    printf( "1 contact: %g %g\n", force[ XX ][ 0 ], force[ YY ][ 0 ] );
      else
    printf( "no contacts\n" );
      return;
    }

  printf( "%d contacts: ", n_contacts );
  for( int i = 0; i < n_contacts; i++ )
    {
      for ( int j = 0; j < N_XYZ; j++ )
    total_force[ j ] += force[ j ][ i ];
      if ( fabsf( force[ XX ][ i ] ) < 1e-6 
       && fabsf( force[ YY ][ i ] ) < 1e-6 )
    continue;
      printf( "%g %g ", force[ XX ][ i ], force[ YY ][ i ] );
    }
  printf( "\n" );
}

/******************************************************************/

void initPhysics( bool interactive )
{
  gFoundation
    = PxCreateFoundation( PX_PHYSICS_VERSION, gAllocator, gErrorCallback );
  gPvd = PxCreatePvd( *gFoundation );
  PxPvdTransport* transport
    = PxDefaultPvdSocketTransportCreate( PVD_HOST, 5425, 10 );
  gPvd->connect( *transport, PxPvdInstrumentationFlag::eALL );
  gPhysics = PxCreatePhysics( PX_PHYSICS_VERSION, *gFoundation,
                  PxTolerancesScale(), true, gPvd );
  PxInitExtensions( *gPhysics, gPvd );
  PxU32 numCores = SnippetUtils::getNbPhysicalCores();
  // gDispatcher = PxDefaultCpuDispatcherCreate( 2 );
  gDispatcher = PxDefaultCpuDispatcherCreate( numCores == 0 ?
                          0 : numCores - 1 );
  PxSceneDesc sceneDesc( gPhysics->getTolerancesScale() );
  sceneDesc.cpuDispatcher = gDispatcher;

  sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);

  // sceneDesc.filterShader = PxDefaultSimulationFilterShader;
  sceneDesc.filterShader = contactReportFilterShader;
  sceneDesc.simulationEventCallback = &gContactReportCallback;  
  gScene = gPhysics->createScene( sceneDesc );
  PxPvdSceneClient* pvdClient = gScene->getScenePvdClient();
  if( pvdClient )
    {
      pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONSTRAINTS, true);
      pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONTACTS, true);
      pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_SCENEQUERIES, true);
    }
  // static friction, dynamic friction, restitution
  gMaterial = gPhysics->createMaterial( 0.9f, 0.9f, 0.0f );

  /*
  gMaterial->setFlag( PxMaterialFlag::eDISABLE_STRONG_FRICTION, true );
  gMaterial->setFlag( PxMaterialFlag::eIMPROVED_PATCH_FRICTION, false );
  gMaterial->setFlag( PxMaterialFlag::eCOMPLIANT_CONTACT, true );
  gMaterial->setRestitution( -1000.0f );
  gMaterial->setDamping( 100.0f );
  */
  PxMaterialFlags gflags = gMaterial->getFlags();
  cout << "Check out PxMaterialFlags: " << (unsigned short) gflags << " " <<
    (unsigned short) PxMaterialFlag::eDISABLE_STRONG_FRICTION << " " <<
    (unsigned short) PxMaterialFlag::eIMPROVED_PATCH_FRICTION << " " <<
    (unsigned short) PxMaterialFlag::eCOMPLIANT_CONTACT << endl;

  PxCombineMode::Enum fcm = gMaterial->getFrictionCombineMode();
  PxCombineMode::Enum rcm = gMaterial->getRestitutionCombineMode();
  cout << "Check out CombineModes " << fcm << " " << rcm << " " <<
    PxCombineMode::eAVERAGE << " " <<
    PxCombineMode::eMIN  << " " <<
    PxCombineMode::eMULTIPLY << " " <<
    PxCombineMode::eMAX << endl;

  // Ground plane causes segmentation fault if angle with vertical >= pi/4
  // PxReal angle = M_PI/4.0 - 1e-6;
  PxReal angle = M_PI/6.0;

  PxRigidStatic* ground_plane
    = PxCreatePlane( *gPhysics, PxPlane( sin( angle ), cos( angle ), 0, 0 ), *gMaterial );
  gScene->addActor( *ground_plane );

  box_body
    = PxCreateDynamic( *gPhysics,
               PxTransform( PxVec3( 0.0,
                        // 1.529*box_half_length,  // Good for angle = M_PI/4.0f
                        2*box_half_length,
                        0.0 ),
                    PxQuat( -angle, PxVec3( 0.0, 0.0, 1.0 ) ) ),
               PxBoxGeometry( 2*box_half_length,
                      box_half_length,
                      box_half_length ),
               *gMaterial, 1.f );
  box_body->setSleepThreshold( 0.0f ); 
  box_body->setLinearDamping( 0.0f );
  box_body->setAngularDamping( 0.0f );
  // Make the motion 2D
  box_body->setRigidDynamicLockFlags( PxRigidDynamicLockFlag::eLOCK_LINEAR_Z
                  | PxRigidDynamicLockFlag::eLOCK_ANGULAR_X
                  | PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y );
  // 2nd arg is mass
  PxRigidBodyExt::setMassAndUpdateInertia( *box_body, 1.0f ); 
  float the_mass = box_body->getMass();
  PxVec3 the_I = box_body->getMassSpaceInertiaTensor();
  printf( "Mass: %g; I: %g %g %g\n", the_mass, the_I[ XX ],
      the_I[ YY ], the_I[ ZZ ] );
  // cout << "mass: " <<  << "; I: " <<  << endl;

  PxVec3 ang_vel( 0.0, 0.0, 0.0 );
  box_body->setAngularVelocity( ang_vel );
  PxVec3 vel( 0.0, 0.0, 0.0 );
  box_body->setLinearVelocity( vel );

  gScene->addActor( *box_body );

  // For acceleration estimation
  for ( int i = 0; i < N_XYZ; i++ )
    last_box_velocity[ i ] = 0;
  last_box_angular_velocity = 0;

  printf( "initPhysics() done.\n" );
}

/******************************************************************/

void cleanupPhysics(bool /*interactive*/)
{
  PX_RELEASE(gScene);
  PX_RELEASE(gDispatcher);
  PxCloseExtensions();
  PX_RELEASE(gPhysics);
  if( gPvd )
    {
      PxPvdTransport* transport = gPvd->getTransport();
      gPvd->release();
      gPvd = NULL;
      PX_RELEASE(transport);
    }
  PX_RELEASE(gFoundation);
  printf( "cleanupPhysics done.\n" );
}

/******************************************************************/
/******************************************************************/
// zero sensors for debugging

void zero_sensors()
{
  box_angle = 0;
  box_angular_acceleration = 0;

  for ( int i = 0; i < N_XYZ; i++ )
    {
      box_velocity[ i ] = 0;
      box_acceleration[ i ] = 0;
      box_angular_velocity[ i ] = 0;
      total_force[ i ] = 0;
      for ( int j = 0; j < MAX_N_CONTACTS; j++ )
    {
      force[ i ][ j ] = 0;
      location[ i ][ j ] = 0;
      normal[ i ][ j ] = 0;
    }
    }
}

/******************************************************************/

void write_data( FILE *stream )
{

  fprintf( stream, "%e %e %e %e %e %e %e %e %e %e ",
       the_time, 
       box_position.p[ XX ], box_position.p[ YY ],
       box_velocity[ XX ], box_velocity[ YY ],
       box_acceleration[ XX ], box_acceleration[ YY ],
       box_angle, box_angular_velocity[ ZZ ],
       box_angular_acceleration );

  fprintf( stream, "%e %e ", total_force[ XX ], total_force[ YY ] );

  fprintf( stream, "\n" );
}

/******************************************************************/
/******************************************************************/

void stepPhysics( bool ignored )
{
  static bool first_time = true;
  static int counter = 0;

  /***********************************************************/
  // Read "sensor" data.

  box_position = box_body->getGlobalPose();
  box_velocity = box_body->getLinearVelocity();

  for ( int i = 0; i < N_XYZ; i++ )
    {
      box_acceleration[ i ]
    = (box_velocity[ i ] - last_box_velocity[ i ])/TIME_STEP;
      last_box_velocity[ i ] = box_velocity[ i ];
    }

  PxVec3 box_angular_velocity = box_body->getAngularVelocity();

  integrated_box_angle
    += ((double) box_angular_velocity[ ZZ ])*TIME_STEP;
  box_angle = integrated_box_angle;

  box_angular_acceleration
    = (box_angular_velocity[ ZZ ] - last_box_angular_velocity)
    /TIME_STEP;

  last_box_angular_velocity = box_angular_velocity[ ZZ ];

  the_time += TIME_STEP;

  gContactPositions.clear();
  gContactNormals.clear();
  gContactImpulses.clear();
  gContactSeparations.clear();

  gScene->simulate( TIME_STEP );
  gScene->fetchResults(true);

  process_contacts();

  write_data( output_stream );

  if ( counter > 1000 )
    {
      fclose( output_stream );
      printf( "Press return to exit.\n" );
      getchar();
      glutLeaveMainLoop();
    }

  counter++;
}

/******************************************************************/
/******************************************************************/

void keyPress(unsigned char key, const PxTransform& camera)
{
  switch(toupper(key))
    {
    case 'X':
      glutLeaveMainLoop();
      break;
    case ' ':
      break;
    default:
      break;
    }
}

/******************************************************************/

int snippetMain(int, const char*const*)
{
  output_stream = fopen( "data", "w" );
  if ( output_stream == NULL )
    {
      fprintf( stderr, "Couldn't open output file\n" );
      exit( -1 );
    }

  extern void renderLoop();
  renderLoop();

  /*
  // For non-rendering version
  static const PxU32 frameCount = 1000;
  initPhysics(false);
  for(PxU32 i=0; i<frameCount; i++)
    stepPhysics(false);
  */

  return 0;
}

/******************************************************************/

Expected Behavior

Fx = 0 Fy = 9.81 (mass = 1)

Actual Behavior

Static forces are wrong. For a block not moving on a 30 degree slope with default PxMaterialFlags Fx = 4.2479 (should be zero) Fy = 7.3575 (should be g, which is set to 9.81, since mass = 1) The overall force magnitude is too big (sqrt(Fx^2 + Fy^2) > 9.81), so it can't be that the forces are reported in some rotated coordinate system. Various combinations of strong friction, improved patch friction, compliant contacts, and removing the 2D constraint (setRigidDynamicLockFlags) did not substantially change these numbers.

Contact forces are also wrong for the dynamic cases of a sphere rolling down a fixed slope or rolling on a curved slope (a larger sphere). However, the accelerations are correct. For the case where the accelerations change (a sphere rolling on a curved slope the contact forces are not simply a scaled version of the correct answer, so there is more than just a scaling factor error going on.

cga-cmu commented 5 months ago

Box2D gets all these cases (static box on slope, cylinder (no 3D spheres in 2D) rolling down a fixed slope, and a cylinder rolling on a curved slope (a larger cylinder) right.

preist-nvidia commented 5 months ago

Thank you for the detailed report and repro, @cga-cmu ! We will investigate. Internal Tracking PX-5048.

farbod-farshidian commented 5 months ago

There is an issue with the current release of Physx. The Contact Reporter only reports the normal force and does not report the friction force. Therefore, the force that it measures is

F = m g cos(\theta) \hat{n}

where \hat{n} is the surface normal. Therefore, the reported force will be

F = m g [cos(\theta) sin(\theta), cos^2(\theta)].

For m=1.0Kg, this will be F = [4.24, 7.35]. Therefore, the reported value is correct.

I have heard that this issue will be fixed in the next release, and the API will report the friction forces.

msauter-nvidia commented 4 months ago

It's as farbod-farshidian is pointing out. The impulses reported in contact reports are only the impulses applied along the contact normals. The next version of PhysX will provide an API to also extract the friction impulses. If then summed up, it will converge to the expected y=9.81 force (plus/minus 0.01 or so) in your example (once it settles down after the initial impact of the falling box).

preist-nvidia commented 4 months ago

@cga-cmu - we expect to release the update with the friction reporting by the end of the month.