Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

report constraint solver analytics data, currently for each island the number of solver iterations used and remaining residual error.

Browse Source
This commit is contained in:
erwincoumans
2019-04-11 22:19:02 -07:00
parent 3146f6276b
commit 5ff52e47d9
11 changed files with 144 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2214,6 +2214,19 @@ B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
return CMD_INVALID_STATUS;
}
B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
//b3Assert(status);
if (status)
{
*analyticsData = status->m_forwardDynamicsAnalyticsArgs;
return status->m_forwardDynamicsAnalyticsArgs.m_numIslands;
}
return 0;
}
B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
{
int numBodies = 0;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -360,6 +360,9 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);

24
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7605,17 +7605,17 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = true;
int numSteps = 0;
if (m_data->m_numSimulationSubSteps > 0)
{
numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
m_data->m_simulationTimestamp += deltaTimeScaled;
m_data->m_simulationTimestamp += deltaTimeScaled;
}
else
{
numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
m_data->m_simulationTimestamp += deltaTimeScaled;
m_data->m_simulationTimestamp += deltaTimeScaled;
}
if (numSteps > 0)
@@ -7624,6 +7624,24 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
}
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps;
btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData;
m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
for (int i=0;i<numIslands;i++)
{
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSolverCalls = islandAnalyticsData[i].m_numSolverCalls;
serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_islandId = islandAnalyticsData[i].m_islandId;
serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numBodies = islandAnalyticsData[i].m_numBodies;
serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numIterationsUsed = islandAnalyticsData[i].m_numIterationsUsed;
serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_remainingLeastSquaresResidual = islandAnalyticsData[i].m_remainingLeastSquaresResidual;
serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numContactManifolds = islandAnalyticsData[i].m_numContactManifolds;
}
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
return hasStatus;

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -957,6 +957,8 @@ struct b3CreateUserShapeArgs
b3CreateUserShapeData m_shapes[MAX_COMPOUND_COLLISION_SHAPES];
};
struct b3CreateUserShapeResultArgs
{
int m_userShapeUniqueId;
@@ -1180,6 +1182,7 @@ struct SharedMemoryStatus
struct SyncUserDataArgs m_syncUserDataArgs;
struct UserDataResponseArgs m_userDataResponseArgs;
struct UserDataRequestArgs m_removeUserDataResponseArgs;
struct b3ForwardDynamicsAnalyticsArgs m_forwardDynamicsAnalyticsArgs;
};
};

20
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -931,6 +931,7 @@ struct b3PhysicsSimulationParameters
int m_minimumSolverIslandSize;
};
enum eConstraintSolverTypes
{
eConstraintSolverLCP_SI = 1,
@@ -941,6 +942,25 @@ enum eConstraintSolverTypes
eConstraintSolverLCP_BLOCK_PGS,
};
struct b3ForwardDynamicsAnalyticsIslandData
{
int m_islandId;
int m_numBodies;
int m_numContactManifolds;
int m_numIterationsUsed;
double m_remainingLeastSquaresResidual;
};
#define MAX_ISLANDS_ANALYTICS 1024
struct b3ForwardDynamicsAnalyticsArgs
{
int m_numSteps;
int m_numIslands;
int m_numSolverCalls;
struct b3ForwardDynamicsAnalyticsIslandData m_islandData[MAX_ISLANDS_ANALYTICS];
};
enum eFileIOActions
{
eAddFileIOAction = 1024,//avoid collision with eFileIOTypes

22
examples/pybullet/pybullet.c
View File

@@ -330,6 +330,28 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
statusHandle = b3SubmitClientCommandAndWaitStatus(
sm, b3InitStepSimulationCommand(sm));
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
{
struct b3ForwardDynamicsAnalyticsArgs analyticsData;
int numIslands = 0;
int i;
numIslands = b3GetStatusForwardDynamicsAnalyticsData(statusHandle, &analyticsData);
PyObject* pyAnalyticsData = PyTuple_New(numIslands);
for (i=0;i<numIslands;i++)
{
int numFields = 4;
PyObject* pyIslandData = PyTuple_New(numFields);
PyTuple_SetItem(pyIslandData, 0, PyLong_FromLong(analyticsData.m_islandData[i].m_islandId));
PyTuple_SetItem(pyIslandData, 1, PyLong_FromLong(analyticsData.m_islandData[i].m_numBodies));
PyTuple_SetItem(pyIslandData, 2, PyLong_FromLong(analyticsData.m_islandData[i].m_numIterationsUsed));
PyTuple_SetItem(pyIslandData, 3, PyFloat_FromDouble(analyticsData.m_islandData[i].m_remainingLeastSquaresResidual));
PyTuple_SetItem(pyAnalyticsData, i, pyIslandData);
}
return pyAnalyticsData;
}
}
}

2
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -64,6 +64,7 @@ struct btContactSolverInfoData
btScalar m_restitutionVelocityThreshold;
bool m_jointFeedbackInWorldSpace;
bool m_jointFeedbackInJointFrame;
bool m_reportSolverAnalytics;
};
struct btContactSolverInfo : public btContactSolverInfoData
@@ -98,6 +99,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution
m_jointFeedbackInWorldSpace = false;
m_jointFeedbackInJointFrame = false;
m_reportSolverAnalytics = false;
}
};

8
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -2239,6 +2239,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
m_analyticsData.m_numSolverCalls++;
m_analyticsData.m_numIterationsUsed = iteration+1;
m_analyticsData.m_islandId = -2;
if (numBodies>0)
m_analyticsData.m_islandId = bodies[0]->getCompanionId();
m_analyticsData.m_numBodies = numBodies;
m_analyticsData.m_numContactManifolds = numManifolds;
m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
break;
}
}

22
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -91,10 +91,29 @@ struct btSISolverSingleIterationData
}
};
struct btSolverAnalyticsData
{
btSolverAnalyticsData()
{
m_numSolverCalls = 0;
m_numIterationsUsed = -1;
m_remainingLeastSquaresResidual = -1;
m_islandId = -2;
}
int m_islandId;
int m_numBodies;
int m_numContactManifolds;
int m_numSolverCalls;
int m_numIterationsUsed;
double m_remainingLeastSquaresResidual;
};
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class)
btSequentialImpulseConstraintSolver : public btConstraintSolver
{
protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
@@ -283,6 +302,8 @@ public:
m_resolveSingleConstraintRowLowerLimit = rowSolver;
}
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
@@ -296,6 +317,7 @@ public:
static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
btSolverAnalyticsData m_analyticsData;
};
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

34
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -207,6 +207,7 @@ public:
}
};
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo* m_solverInfo;
@@ -224,6 +225,8 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: m_solverInfo(NULL),
@@ -244,6 +247,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
m_islandAnalyticsData.clear();
btAssert(solverInfo);
m_solverInfo = solverInfo;
@@ -270,6 +274,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_solverInfo->m_reportSolverAnalytics)
{
m_solver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
}
else
{
@@ -335,7 +344,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
processConstraints(islandId);
}
else
{
@@ -344,7 +353,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
}
}
}
void processConstraints()
void processConstraints(int islandId=-1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
@@ -354,6 +363,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_bodies.size() && m_solverInfo->m_reportSolverAnalytics)
{
m_solver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
@@ -361,6 +375,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
}
};
void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
{
islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
}
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver)
@@ -720,10 +739,13 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
if (!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
if (bod->internalNeedsJointFeedback())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
}

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -109,5 +109,7 @@ public:
virtual void serialize(btSerializer* serializer);
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H