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

Merge pull request #2203 from erwincoumans/master

Browse Source 
implement stablePD control version of testLaikago,  fix getCameraImage in VR,  only report solver analytics if enabled using setPhysicsEngineParameter
This commit is contained in:
erwincoumans
2019-04-15 08:52:03 -07:00
committed by GitHub
parent c6a43e0a5b e97a7d77af
commit a929e8f2e5
16 changed files with 857 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -64,6 +64,7 @@ struct btContactSolverInfoData
btScalar m_restitutionVelocityThreshold;
bool m_jointFeedbackInWorldSpace;
bool m_jointFeedbackInJointFrame;
int 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 = 0;
}
};

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&1)
{
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&1))
{
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)
@@ -721,10 +740,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