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

implement getJointInfo for objects loaded through SDF

Browse Source
This commit is contained in:
Erwin Coumans
2016-06-15 18:01:41 -07:00
parent 456c844a6b
commit 95a09bec99
10 changed files with 122 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -26,7 +26,7 @@ public:
virtual int getNumJoints(int bodyIndex) const = 0;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

4
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -653,10 +653,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
}
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(bodyIndex, linkIndex,*info);
return cl->getJointInfo(bodyIndex, linkIndex,*info);
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -57,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h

99
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -37,6 +37,9 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus;
int m_counter;
@@ -82,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
}
void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
return false;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -167,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() {
return true;
}
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
SharedMemoryStatus* stat = 0;
@@ -208,6 +254,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("Server loading the SDF OK\n");
}
break;
}
case CMD_URDF_LOADING_COMPLETED: {
@@ -275,6 +322,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_BODY_INFO_COMPLETED:
{
if (m_data->m_verboseOutput) {
b3Printf("Received body info\n");
}
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyUniqueId, serverCmd);
break;
}
case CMD_SDF_LOADING_FAILED: {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the SDF...\n");
@@ -504,21 +561,49 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_waitingForServer = true;
}
/*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)
{
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
for (int i=0;i<numBodies;i++)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
}
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_SDF_INFO;
command.m_updateFlags = SDF_REQUEST_INFO_BODY;
command.m_sdfRequestInfoArgs.m_infoIndex = 0;
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
}
}
*/
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{
//are there any bodies left to be processed?
if (m_data->m_bodyIdsRequestInfo.size())
{
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{

4
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -11,6 +11,8 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsClientSharedMemory();
@@ -34,7 +36,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

9
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -393,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const
return 0;
}
void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
if (jointIndex < bodyJoints->m_jointInfo.size())
{
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
}
return false;
}
///todo: move this out of the

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -48,7 +48,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

4
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const
return m_data->m_physicsClient->getNumJoints(bodyIndex);
}
void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
}
///todo: move this out of the

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -40,7 +40,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

14
test/SharedMemory/test.c
View File

@@ -65,8 +65,18 @@ void testSharedMemory(b3PhysicsClientHandle sm)
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
numJoints = b3GetNumJoints(sm,bodyIndicesOut[0]);
printf("numJoints: %d\n", numJoints);
int bodyUniqueId = bodyIndicesOut[0];
numJoints = b3GetNumJoints(sm,bodyUniqueId);
b3Printf("numJoints: %d\n", numJoints);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
{
b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
}
}
//ASSERT_EQ(numBodies ==1);
}