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

expose getNumBodies, getBodyUniqueId, getBodyInfo (char* baseName) to shared memory API and pybullet., making it easier to serialize the state of the world.

Browse Source
This commit is contained in:
erwin coumans
2016-09-27 12:13:45 -07:00
parent a325747dd4
commit 0936ae6600
13 changed files with 250 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SharedMemory/PhysicsClient.h
View File

@@ -24,9 +24,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
virtual int getNumJoints(int bodyIndex) const = 0;
virtual int getNumBodies() const = 0;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getBodyUniqueId(int serialIndex) const = 0;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0;
virtual int getNumJoints(int bodyUniqueId) const = 0;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

23
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -739,6 +739,29 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
}
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumBodies();
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getBodyUniqueId(serialIndex);
}
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getBodyInfo(bodyUniqueId,*info);
}
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -53,6 +53,15 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -624,7 +624,7 @@ void PhysicsClientExample::initPhysics()
{
MyCallback(CMD_LOAD_URDF, true, this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_RESET_SIMULATION,true,this);
}

45
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -18,6 +18,7 @@
struct BodyJointInfoCache
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
@@ -74,10 +75,37 @@ struct PhysicsClientSharedMemoryInternalData {
int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
int PhysicsClientSharedMemory::getNumBodies() const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
return m_data->m_bodyJointMap.size();
}
int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
{
return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
return true;
}
return false;
}
int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
@@ -88,9 +116,9 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
}
bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
@@ -196,12 +224,13 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
@@ -272,10 +301,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++) {

10
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -34,9 +34,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumJoints(int bodyIndex) const;
virtual int getNumBodies() const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyUniqueId) const;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

40
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -9,10 +9,11 @@
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
#include "BodyJointInfoUtility.h"
#include <string>
struct BodyJointInfoCache2
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
@@ -329,8 +330,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
@@ -341,13 +341,15 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
@@ -458,6 +460,34 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return hasStatus;
}
int PhysicsDirect::getNumBodies() const
{
return m_data->m_bodyJointMap.size();
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
{
return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
return true;
}
return false;
}
int PhysicsDirect::getNumJoints(int bodyIndex) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -49,6 +49,12 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

15
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -74,6 +74,21 @@ bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& comm
return m_data->m_physicsClient->submitClientCommand(command);
}
int PhysicsLoopBack::getNumBodies() const
{
return m_data->m_physicsClient->getNumBodies();
}
int PhysicsLoopBack::getBodyUniqueId(int serialIndex) const
{
return m_data->m_physicsClient->getBodyUniqueId(serialIndex);
}
bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info);
}
int PhysicsLoopBack::getNumJoints(int bodyIndex) const
{
return m_data->m_physicsClient->getNumJoints(bodyIndex);

6
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -38,6 +38,12 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

4
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1064,6 +1064,10 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
if (mb->getBaseName())
{
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
}
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -117,6 +117,13 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3BodyInfo
{
const char* m_baseName;
};
struct b3JointSensorState
{
double m_jointPosition;