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

allow to control of powered joints after loading a URDF file, through shared memory

Browse Source 
more refactor of shared memory joint control API
This commit is contained in:
Erwin Coumans
2015-07-20 23:35:29 -07:00
parent 68b53feb9c
commit 8e163c984d
10 changed files with 279 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -8,15 +8,26 @@
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
//copied from btMultiBodyLink.h
enum JointType
{
eRevoluteType = 0,
ePrismaticType = 1,
};
struct PhysicsClientSharedMemoryInternalData
{
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<PoweredJointInfo> m_poweredJointInfo;
int m_counter;
bool m_serverLoadUrdfOK;
bool m_isConnected;
bool m_waitingForServer;
bool m_hasLastServerStatus;
PhysicsClientSharedMemoryInternalData()
:m_sharedMemory(0),
@@ -24,7 +35,8 @@ struct PhysicsClientSharedMemoryInternalData
m_counter(0),
m_serverLoadUrdfOK(false),
m_isConnected(false),
m_waitingForServer(false)
m_waitingForServer(false),
m_hasLastServerStatus(false)
{
}
@@ -37,6 +49,18 @@ struct PhysicsClientSharedMemoryInternalData
};
int PhysicsClientSharedMemory::getNumPoweredJoints() const
{
return m_data->m_poweredJointInfo.size();
}
void PhysicsClientSharedMemory::getPoweredJointInfo(int index, PoweredJointInfo& info) const
{
info = m_data->m_poweredJointInfo[index];
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{
@@ -99,16 +123,19 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
void PhysicsClientSharedMemory::processServerStatus()
bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus)
{
btAssert(m_data->m_testBlock1);
bool hasStatus = false;
if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
hasStatus = true;
serverStatus = serverCmd;
//consume the command
switch (serverCmd.m_type)
{
@@ -123,10 +150,14 @@ void PhysicsClientSharedMemory::processServerStatus()
bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf->setFileDNAisMemoryDNA();
bf->parse(false);
m_data->m_robotMultiBodyData.push_back(bf);
for (int i=0;i<bf->m_multiBodies.size();i++)
{
int flag = bf->getFlags();
int qOffset = 7;
int uOffset=6;
if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
{
Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
@@ -136,15 +167,31 @@ void PhysicsClientSharedMemory::processServerStatus()
}
for (int link=0;link<mb->m_numLinks;link++)
{
if (mb->m_links[link].m_linkName)
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
}
if (mb->m_links[link].m_jointName)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
PoweredJointInfo info;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
if (mb->m_links[link].m_linkName)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
info.m_linkName = mb->m_links[link].m_linkName;
}
if (mb->m_links[link].m_jointName)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
m_data->m_poweredJointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
}
} else
{
Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
@@ -154,19 +201,39 @@ void PhysicsClientSharedMemory::processServerStatus()
}
for (int link=0;link<mb->m_numLinks;link++)
{
if (mb->m_links[link].m_linkName)
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
}
b3Printf("link [%d] type = %d",link, mb->m_links[link].m_jointType);
if (mb->m_links[link].m_jointName)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
PoweredJointInfo info;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
if (mb->m_links[link].m_linkName)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
info.m_linkName = mb->m_links[link].m_linkName;
}
if (mb->m_links[link].m_jointName)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
m_data->m_poweredJointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
}
}
}
printf("ok!\n");
if (bf->ok())
{
b3Printf("Received robot description ok!\n");
} else
{
b3Warning("Robot description not received");
}
}
break;
}
@@ -212,21 +279,42 @@ void PhysicsClientSharedMemory::processServerStatus()
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
char msg[1024];
sprintf(msg,"Q=[");
for (int i=0;i<numQ;i++)
{
if (i<numQ-1)
sprintf(msg,"Q=[");
for (int i=0;i<numQ;i++)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
}
}
}
sprintf(msg,"%s]",msg);
sprintf(msg,"%s]",msg);
}
b3Printf(msg);
{
sprintf(msg,"U=[");
for (int i=0;i<numU;i++)
{
if (i<numU-1)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
}
}
sprintf(msg,"%s]",msg);
}
b3Printf(msg);
b3Printf("\n");
break;
}
@@ -250,6 +338,7 @@ void PhysicsClientSharedMemory::processServerStatus()
m_data->m_waitingForServer = true;
}
}
return hasStatus;
}
bool PhysicsClientSharedMemory::canSubmitCommand() const
@@ -378,7 +467,7 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
{
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
// m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_data->m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++);
} else