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

Merge pull request #766 from erwincoumans/master

Browse Source 
[SharedMemory] Calculate inverse dynamics now uses world gravity.
This commit is contained in:
erwincoumans
2016-09-01 14:37:50 -07:00
committed by GitHub
parent 552da3adb5 fe46ee8adf
commit 03f09042e2
19 changed files with 424 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -40,6 +40,8 @@ public:
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
};
#endif // BT_PHYSICS_CLIENT_API_H

45
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1103,6 +1103,51 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
}
}
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION;
command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedContactPointInformation(contactPointData);
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -80,6 +80,11 @@ void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command,
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);

23
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -274,7 +274,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-1.5);
b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
@@ -415,6 +415,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
if (m_selectedBody >= 0)
@@ -442,6 +443,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
b3SetContactFilterBodyA(commandHandle,0);
b3SetContactFilterBodyB(commandHandle,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
@@ -530,6 +539,7 @@ void PhysicsClientExample::createButtons()
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger);
if (m_bodyUniqueIds.size())
{
@@ -946,6 +956,17 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
}
if (statusType == CMD_CONTACT_POINT_INFORMATION_FAILED)
{
b3Warning("Cannot get contact information");
}
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
b3ContactInformation contactPointData;
b3GetContactPointInformation(m_physicsClientHandle, &contactPointData);
b3Printf("Num Contacts: %d\n", contactPointData.m_numContactPoints);
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))

54
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -38,6 +38,9 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3ContactPointDynamicsData>m_cachedContactPointDynamics;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -58,6 +61,7 @@ struct PhysicsClientSharedMemoryInternalData {
m_counter(0),
m_cachedCameraPixelsWidth(0),
m_cachedCameraPixelsHeight(0),
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(false),
@@ -558,6 +562,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Inverse Dynamics computations failed");
break;
}
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Contact Point Information Request OK\n");
}
int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
m_data->m_cachedContactPointDynamics.resize(0);
b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0;i<numContactsCopied;i++)
{
m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
}
break;
}
case CMD_CONTACT_POINT_INFORMATION_FAILED:
{
b3Warning("Contact Point Information Request failed");
break;
}
default: {
b3Error("Unknown server status\n");
btAssert(0);
@@ -620,6 +651,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
//todo: request remaining points, if needed
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -716,6 +762,14 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
}
void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
contactPointData->m_contactDynamicsData = 0;
contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {
return &m_data->m_debugLinesFrom[0].m_x;

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -48,6 +48,8 @@ public:
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
};
#endif // BT_PHYSICS_CLIENT_API_H

9
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -487,3 +487,12 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
}
}
void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
if (contactPointData)
{
contactPointData->m_numContactPoints = 0;
}
}

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -64,6 +64,8 @@ public:
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();

7
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -105,10 +105,12 @@ const float* PhysicsLoopBack::getDebugLinesFrom() const
{
return m_data->m_physicsClient->getDebugLinesFrom();
}
const float* PhysicsLoopBack::getDebugLinesTo() const
{
return m_data->m_physicsClient->getDebugLinesTo();
}
const float* PhysicsLoopBack::getDebugLinesColor() const
{
return m_data->m_physicsClient->getDebugLinesColor();
@@ -118,3 +120,8 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
{
return m_data->m_physicsClient->getCachedCameraImage(cameraData);
}
void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
}

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -53,6 +53,8 @@ public:
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
};

183
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -403,6 +403,8 @@ struct PhysicsServerCommandProcessorInternalData
btDefaultCollisionConfiguration* m_collisionConfiguration;
btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
@@ -507,7 +509,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
}
m_data->m_guiHelper = guiHelper;
}
@@ -518,7 +520,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
}
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -759,6 +761,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
u2b.setBodyUniqueId(bodyUniqueId);
{
btScalar mass = 0;
@@ -782,9 +786,16 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
mb = creation.getBulletMultiBody();
rb = creation.getRigidBody();
if (rb)
rb->setUserIndex2(bodyUniqueId);
if (mb)
mb->setUserIndex2(bodyUniqueId);
if (mb)
{
bodyHandle->m_multiBody = mb;
m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
@@ -856,6 +867,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
{
btScalar mass = 0;
@@ -887,14 +900,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody();
if (useMultiBody)
{
if (mb)
{
mb->setUserIndex2(bodyUniqueId);
bodyHandle->m_multiBody = mb;
createJointMotors(mb);
@@ -958,6 +971,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (rb)
{
bodyHandle->m_rigidBody = rb;
rb->setUserIndex2(bodyUniqueId);
return true;
}
}
@@ -1024,7 +1038,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
bool hasStatus = false;
{
@@ -1154,7 +1168,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
int numPixelsCopied = 0;
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
{
@@ -1481,10 +1495,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
}
}
motor->setVelocityTarget(desiredVelocity,kd);
btScalar kp = 0.f;
motor->setPositionTarget(0,kp);
hasDesiredVelocity = true;
@@ -1565,7 +1579,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
motor->setVelocityTarget(desiredVelocity,kd);
motor->setPositionTarget(desiredPosition,kp);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
@@ -1805,7 +1819,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_STEP_FORWARD_SIMULATION:
{
if (m_data->m_verboseOutput)
{
b3Printf("Step simulation request");
@@ -1897,7 +1911,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
mb->setWorldToBaseRot(invOrn.inverse());
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
@@ -1936,10 +1950,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_visualConverter.resetAll();
}
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
@@ -2079,6 +2093,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
rb->setUserIndex2(bodyUniqueId);
bodyHandle->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rigidBody = rb;
hasStatus = true;
@@ -2121,6 +2136,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
//make a snapshot of the contact manifolds into individual contact points
if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0)
{
int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
m_data->m_cachedContactPoints.resize(0);
m_data->m_cachedContactPoints.reserve(numContactManifolds*4);
for (int i=0;i<numContactManifolds;i++)
{
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
int objectIndexB = -1;
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
if (bodyB)
{
objectIndexB = bodyB->getUserIndex2();
}
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
if (mblB && mblB->m_multiBody)
{
objectIndexB = mblB->m_multiBody->getUserIndex2();
}
int objectIndexA = -1;
const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
if (bodyA)
{
objectIndexA = bodyA->getUserIndex2();
}
const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
if (mblA && mblA->m_multiBody)
{
objectIndexA = mblA->m_multiBody->getUserIndex2();
}
btAssert(bodyA || mblA);
//apply the filter, if any
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
continue;
}
if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
continue;
}
for (int p=0;p<manifold->getNumContacts();p++)
{
//if the point passes the optional filter, add it
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
{
//one of the two unique Ids has to match...
}
b3ContactPointData pt;
pt.m_bodyUniqueIdA = -1;
pt.m_bodyUniqueIdB = -1;
const btManifoldPoint& srcPt = manifold->getContactPoint(p);
pt.m_contactDistance = srcPt.getDistance();
pt.m_contactFlags = 0;
pt.m_contactPointDynamicsIndex = -1;
pt.m_linkIndexA = -1;
pt.m_linkIndexB = -1;
for (int j=0;j<3;j++)
{
pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
m_data->m_cachedContactPoints.push_back (pt);
}
}
}
int numContactPoints = m_data->m_cachedContactPoints.size();
//b3ContactPoint
//struct b3ContactPointDynamics
int totalBytesPerContact = sizeof(b3ContactPointData);
int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1;
b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
int numContactPointBatch = btMin(numContactPoints,contactPointStorage);
int endContactPointIndex = startContactPointIndex+numContactPointBatch;
for (int i=startContactPointIndex;i<endContactPointIndex ;i++)
{
const b3ContactPointData& srcPt = m_data->m_cachedContactPoints[i];
b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
destPt = srcPt;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
}
serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED,
hasStatus = true;
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
@@ -2128,7 +2261,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree** treePtrPtr =
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
@@ -2140,12 +2273,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
{
b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
else
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
@@ -2163,8 +2296,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) &&
-1 != tree->setGravityInWorldFrame(id_grav))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
@@ -2179,7 +2314,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
}
}
else
{
@@ -2203,7 +2338,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btMultiBody* mb = body->m_multiBody;
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
@@ -2213,7 +2348,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_externalForceArguments.m_positions[i*3+0],
clientCmd.m_externalForceArguments.m_positions[i*3+1],
clientCmd.m_externalForceArguments.m_positions[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
@@ -2236,7 +2371,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
@@ -2252,7 +2387,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -144,6 +144,12 @@ enum EnumRequestPixelDataUpdateFlags
};
struct RequestContactDataArgs
{
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
};
struct SendDebugLinesArgs
@@ -406,6 +412,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
};
};
@@ -414,6 +421,13 @@ struct RigidBodyCreateArgs
int m_bodyUniqueId;
};
struct SendContactDataArgs
{
int m_startingContactPointIndex;
int m_numContactPointsCopied;
int m_numRemainingContactPoints;
};
struct SharedMemoryStatus
{
int m_type;
@@ -430,6 +444,7 @@ struct SharedMemoryStatus
struct SendPixelDataArgs m_sendPixelDataArguments;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
};
};

38
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -29,8 +29,11 @@ enum EnumSharedMemoryClientCommand
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_CALCULATE_INVERSE_KINEMATICS,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
CMD_CREATE_JOINT
};
enum EnumSharedMemoryServerStatus
@@ -64,6 +67,8 @@ enum EnumSharedMemoryServerStatus
CMD_INVALID_STATUS,
CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_MAX_SERVER_COMMANDS
};
@@ -131,6 +136,37 @@ struct b3CameraImageData
const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
};
struct b3ContactPointData
{
int m_contactFlags;//flag wether certain fields below are valid
int m_bodyUniqueIdA;
int m_bodyUniqueIdB;
int m_linkIndexA;
int m_linkIndexB;
double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
double m_contactDistance;//negative number is penetration, positive is distance.
int m_contactPointDynamicsIndex;
};
struct b3ContactPointDynamicsData
{
double m_normalForce;
double m_linearFrictionDirection[3];
double m_linearFrictionForce;
double m_angularFrictionDirection[3];
double m_angularFrictionForce;
};
struct b3ContactInformation
{
int m_numContactPoints;
struct b3ContactPointData* m_contactPointData;
struct b3ContactPointDynamicsData* m_contactDynamicsData;
};
///b3LinkState provides extra information such as the Cartesian world coordinates
///center of mass (COM) of the link, relative to the world reference frame.
///Orientation is a quaternion x,y,z,w

6
examples/TinyRenderer/tgaimage.h
View File

@@ -30,20 +30,20 @@ struct TGAColor {
bgra[i] = 0;
}
TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) {
TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bytespp(4) {
bgra[0] = B;
bgra[1] = G;
bgra[2] = R;
bgra[3] = A;
}
TGAColor(unsigned char v) : bgra(), bytespp(1) {
TGAColor(unsigned char v) : bytespp(1) {
for (int i=0; i<4; i++) bgra[i] = 0;
bgra[0] = v;
}
TGAColor(const unsigned char *p, unsigned char bpp) : bgra(), bytespp(bpp) {
TGAColor(const unsigned char *p, unsigned char bpp) : bytespp(bpp) {
for (int i=0; i<(int)bpp; i++) {
bgra[i] = p[i];
}

8
examples/pybullet/pybullet.c
View File

@@ -617,14 +617,16 @@ static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
return 0;
}
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
b3GetStatusActualState(status_handle, 0/* body_unique_id */,
0/* num_degree_of_freedom_q */,
0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/,

1
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
View File

@@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject()
m_rollingFriction(0.0f),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_hitFraction(btScalar(1.)),
m_ccdSweptSphereRadius(btScalar(0.)),

17
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -93,8 +93,10 @@ protected:
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
void* m_userObjectPointer;
void* m_userObjectPointer;
int m_userIndex2;
int m_userIndex;
///time of impact calculation
@@ -476,6 +478,12 @@ public:
{
return m_userIndex;
}
int getUserIndex2() const
{
return m_userIndex2;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
{
@@ -487,6 +495,11 @@ public:
{
m_userIndex = index;
}
void setUserIndex2(int index)
{
m_userIndex2 = index;
}
int getUpdateRevisionInternal() const
{

4
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -106,7 +106,9 @@ btMultiBody::btMultiBody(int n_links,
m_awake(true),
m_canSleep(canSleep),
m_sleepTimer(0),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true),

36
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -577,6 +577,38 @@ void addJointTorque(int i, btScalar Q);
m_baseName = name;
}
///users can point to their objects, userPointer is not used by Bullet
void* getUserPointer() const
{
return m_userObjectPointer;
}
int getUserIndex() const
{
return m_userIndex;
}
int getUserIndex2() const
{
return m_userIndex2;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
{
m_userObjectPointer = userPointer;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserIndex(int index)
{
m_userIndex = index;
}
void setUserIndex2(int index)
{
m_userIndex2 = index;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@@ -653,6 +685,10 @@ private:
bool m_canSleep;
btScalar m_sleepTimer;
void* m_userObjectPointer;
int m_userIndex2;
int m_userIndex;
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;