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

initial support for multiple robots in shared memory API

Browse Source
This commit is contained in:
erwin coumans
2015-10-13 11:32:25 -07:00
parent 8d26ff356d
commit 4a29986662
15 changed files with 598 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -24,9 +24,9 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
virtual int getNumJoints() const = 0;
virtual int getNumJoints(int bodyIndex) const = 0;
virtual void getJointInfo(int index, struct b3JointInfo& info) const = 0;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

50
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -135,6 +135,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -197,6 +198,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -204,12 +206,17 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3JointInfo info;
b3GetJointInfo(physClient, jointIndex, &info);
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0);
if (bodyIndex>=0)
{
b3JointInfo info;
b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
}
}
@@ -279,6 +286,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
command->m_createSensorArguments.m_bodyUniqueId = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -349,7 +357,29 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
return 0;
}
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyId = -1;
if (status)
{
switch (status->m_type)
{
case CMD_URDF_LOADING_COMPLETED:
{
bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
break;
}
default:
{
b3Assert(0);
}
};
}
return bodyId;
}
int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -379,17 +409,17 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
}
int b3GetNumJoints(b3PhysicsClientHandle physClient)
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumJoints();
return cl->getNumJoints(bodyId);
}
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info)
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(linkIndex,*info);
cl->getJointInfo(bodyIndex, linkIndex,*info);
}
int b3PickBody(struct SharedMemoryCommand *command,

8
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -34,10 +34,12 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetNumJoints(b3PhysicsClientHandle physClient);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info);
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);

150
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -29,7 +29,8 @@ protected:
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
void createButton(const char* name, int id, bool isTrigger );
void createButtons();
@@ -45,12 +46,22 @@ public:
virtual ~PhysicsClientExample();
virtual void initPhysics();
void selectComboBox(int comboIndex, const char* name)
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
int bodyIndex = comboIndex;
if (m_selectedBody != bodyIndex)
{
m_selectedBody = bodyIndex;
}
}
}
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float dist = 5;
float pitch = 50;
float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
@@ -144,13 +155,27 @@ public:
};
void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
{
b3Printf("Item selected %s", item);
PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
b3Assert(cl);
if (cl)
{
cl->selectComboBox(combobox,item);
}
}
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
if (buttonState)
b3Assert(cl);
if (cl && buttonState)
{
cl->enqueueCommand(buttonId);
}
@@ -173,9 +198,10 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
double startPosY = 0;
static double startPosY = 0;
double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
@@ -252,6 +278,11 @@ m_numMotors(0)
PhysicsClientExample::~PhysicsClientExample()
{
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
b3Printf("~PhysicsClientExample\n");
}
@@ -269,6 +300,8 @@ void PhysicsClientExample::createButtons()
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->removeAllParameters();
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
@@ -276,9 +309,44 @@ void PhysicsClientExample::createButtons()
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
if (m_selectedBody>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,m_selectedBody,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
}
}
}
}
}
}
void PhysicsClientExample::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
@@ -296,6 +364,9 @@ void PhysicsClientExample::initPhysics()
MyCallback(CMD_RESET_SIMULATION,true,this);
}
m_selectedBody = -1;
m_prevSelectedBody = -1;
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
@@ -307,6 +378,11 @@ void PhysicsClientExample::initPhysics()
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
@@ -317,43 +393,34 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle);
for (int i=0;i<numJoints;i++)
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
comboParams.m_numItems = 1;
comboParams.m_startItem = 0;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
const char* bla = "bla";
const char* blarray[1];
blarray[0] = bla;
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
}
}
}
}
}
}
@@ -375,10 +442,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (commandId ==CMD_RESET_SIMULATION)
{
if (m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->removeAllParameters();
}
m_selectedBody = -1;
m_numMotors=0;
createButtons();
}

59
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -30,12 +30,17 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z) {
return tmp;
}
struct BodyJointInfoCache
{
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
struct PhysicsClientSharedMemoryInternalData {
SharedMemoryInterface* m_sharedMemory;
SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
@@ -66,10 +71,29 @@ struct PhysicsClientSharedMemoryInternalData {
bool canSubmitCommand() const;
};
int PhysicsClientSharedMemory::getNumJoints() const { return m_data->m_jointInfo.size(); }
int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const {
info = m_data->m_jointInfo[index];
return bodyJoints->m_jointInfo.size();
}
btAssert(0);
return 0;
}
void 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];
}
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -169,7 +193,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf->setFileDNAisMemoryDNA();
bf->parse(false);
m_data->m_robotMultiBodyData.push_back(bf);
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
for (int i = 0; i < bf->m_multiBodies.size(); i++) {
int flag = bf->getFlags();
@@ -214,7 +241,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
(mb->m_links[link].m_jointType == ePrismaticType)) {
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
bodyJoints->m_jointInfo.push_back(info);
}
qOffset += mb->m_links[link].m_posVarCount;
uOffset += mb->m_links[link].m_dofCount;
@@ -257,7 +284,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
(mb->m_links[link].m_jointType == ePrismaticType)) {
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
bodyJoints->m_jointInfo.push_back(info);
}
qOffset += mb->m_links[link].m_posVarCount;
uOffset += mb->m_links[link].m_dofCount;
@@ -367,12 +394,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
}
for (int i = 0; i < m_data->m_robotMultiBodyData.size(); i++) {
delete m_data->m_robotMultiBodyData[i];
}
m_data->m_robotMultiBodyData.clear();
m_data->m_jointInfo.clear();
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
break;
}
case CMD_DEBUG_LINES_COMPLETED: {

4
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -30,9 +30,9 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumJoints() const;
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int index, struct b3JointInfo& info) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

261
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -74,14 +74,136 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
m_lines2.push_back(line);
}
};
struct InteralBodyData
{
btMultiBody* m_multiBody;
int m_testData;
btTransform m_rootLocalInertialFrame;
InteralBodyData()
:m_multiBody(0),
m_testData(0)
{
m_rootLocalInertialFrame.setIdentity();
}
};
///todo: templatize this
struct InternalBodyHandle : public InteralBodyData
{
BT_DECLARE_ALIGNED_ALLOCATOR();
int m_nextFreeHandle;
void SetNextFree(int next)
{
m_nextFreeHandle = next;
}
int GetNextFree() const
{
return m_nextFreeHandle;
}
};
struct PhysicsServerInternalData
{
///handle management
btAlignedObjectArray<InternalBodyHandle> m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
InternalBodyHandle* getHandle(int handle)
{
btAssert(handle>=0);
btAssert(handle<m_bodyHandles.size());
if ((handle<0) || (handle>=m_bodyHandles.size()))
{
return 0;
}
return &m_bodyHandles[handle];
}
const InternalBodyHandle* getHandle(int handle) const
{
return &m_bodyHandles[handle];
}
void increaseHandleCapacity(int extraCapacity)
{
int curCapacity = m_bodyHandles.size();
btAssert(curCapacity == m_numUsedHandles);
int newCapacity = curCapacity + extraCapacity;
m_bodyHandles.resize(newCapacity);
{
for (int i = curCapacity; i < newCapacity; i++)
m_bodyHandles[i].SetNextFree(i + 1);
m_bodyHandles[newCapacity - 1].SetNextFree(-1);
}
m_firstFreeHandle = curCapacity;
}
void initHandles()
{
m_numUsedHandles = 0;
m_firstFreeHandle = -1;
increaseHandleCapacity(1);
}
void exitHandles()
{
m_bodyHandles.resize(0);
m_firstFreeHandle = -1;
m_numUsedHandles = 0;
}
int allocHandle()
{
btAssert(m_firstFreeHandle>=0);
int handle = m_firstFreeHandle;
m_firstFreeHandle = getHandle(handle)->GetNextFree();
m_numUsedHandles++;
if (m_firstFreeHandle<0)
{
int curCapacity = m_bodyHandles.size();
int additionalCapacity= m_bodyHandles.size();
increaseHandleCapacity(additionalCapacity);
getHandle(handle)->SetNextFree(m_firstFreeHandle);
}
return handle;
}
void freeHandle(int handle)
{
btAssert(handle >= 0);
getHandle(handle)->SetNextFree(m_firstFreeHandle);
m_firstFreeHandle = handle;
m_numUsedHandles--;
}
///end handle management
SharedMemoryInterface* m_sharedMemory;
SharedMemoryBlock* m_testBlock1;
bool m_isConnected;
btScalar m_physicsDeltaTime;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
@@ -95,7 +217,7 @@ struct PhysicsServerInternalData
btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_debugDrawer;
btTransform m_rootLocalInertialFrame;
struct GUIHelperInterface* m_guiHelper;
@@ -127,7 +249,51 @@ struct PhysicsServerInternalData
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
{
m_rootLocalInertialFrame.setIdentity();
initHandles();
#if 0
btAlignedObjectArray<int> bla;
for (int i=0;i<1024;i++)
{
int handle = allocHandle();
bla.push_back(handle);
InternalBodyHandle* body = getHandle(handle);
InteralBodyData* body2 = body;
}
for (int i=0;i<bla.size();i++)
{
freeHandle(bla[i]);
}
bla.resize(0);
for (int i=0;i<1024;i++)
{
int handle = allocHandle();
bla.push_back(handle);
InternalBodyHandle* body = getHandle(handle);
InteralBodyData* body2 = body;
}
for (int i=0;i<bla.size();i++)
{
freeHandle(bla[i]);
}
bla.resize(0);
for (int i=0;i<1024;i++)
{
int handle = allocHandle();
bla.push_back(handle);
InternalBodyHandle* body = getHandle(handle);
InteralBodyData* body2 = body;
}
for (int i=0;i<bla.size();i++)
{
freeHandle(bla[i]);
}
#endif
}
SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp)
@@ -422,7 +588,7 @@ void PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb)
bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase)
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr)
{
btAssert(m_data->m_dynamicsWorld);
if (!m_data->m_dynamicsWorld)
@@ -437,12 +603,19 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
if (loadOk)
{
//get a body index
int bodyUniqueId = m_data->allocHandle();
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
{
btScalar mass = 0;
m_data->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rootLocalInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0,0,0);
int urdfLinkIndex = u2b.getRootLinkIndex();
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,m_data->m_rootLocalInertialFrame);
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
}
if (m_data->m_verboseOutput)
{
@@ -463,6 +636,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
{
if (mb)
{
bodyHandle->m_multiBody = mb;
createJointMotors(mb);
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
@@ -636,13 +810,7 @@ void PhysicsServerSharedMemory::processClientCommands()
}
case CMD_LOAD_URDF:
{
//at the moment, we only load 1 urdf / robot
if (m_data->m_urdfLinkNameMapper.size())
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
break;
}
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
if (m_data->m_verboseOutput)
{
@@ -667,21 +835,23 @@ void PhysicsServerSharedMemory::processClientCommands()
}
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true;
bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false;
int bodyUniqueId;
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn,
useMultiBody, useFixedBase);
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
useMultiBody, useFixedBase,&bodyUniqueId);
if (completedOk)
{
if (m_data->m_urdfLinkNameMapper.size())
{
serverCmd.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
if (m_data->m_urdfLinkNameMapper.size())
{
status.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
status.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
}
m_data->submitServerStatus(status);
} else
@@ -701,10 +871,11 @@ void PhysicsServerSharedMemory::processClientCommands()
{
b3Printf("Processed CMD_CREATE_SENSOR");
}
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btMultiBody* mb = body->m_multiBody;
btAssert(mb);
for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++)
{
@@ -767,9 +938,13 @@ void PhysicsServerSharedMemory::processClientCommands()
{
b3Printf("Processed CMD_SEND_DESIRED_STATE");
}
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btMultiBody* mb = body->m_multiBody;
btAssert(mb);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
@@ -813,9 +988,7 @@ void PhysicsServerSharedMemory::processClientCommands()
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link=0;link<mb->getNumLinks();link++)
{
@@ -851,9 +1024,7 @@ void PhysicsServerSharedMemory::processClientCommands()
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for (int link=0;link<mb->getNumLinks();link++)
@@ -916,15 +1087,23 @@ void PhysicsServerSharedMemory::processClientCommands()
{
b3Printf("Sending the actual state (Q,U)");
}
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
btMultiBody* mb = 0;
if (body)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
mb = body->m_multiBody;
}
if (mb)
{
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
@@ -934,20 +1113,20 @@ void PhysicsServerSharedMemory::processClientCommands()
tr.setRotation(mb->getWorldToBaseRot().inverse());
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
m_data->m_rootLocalInertialFrame.getOrigin()[0];
body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
m_data->m_rootLocalInertialFrame.getOrigin()[1];
body->m_rootLocalInertialFrame.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
m_data->m_rootLocalInertialFrame.getOrigin()[2];
body->m_rootLocalInertialFrame.getOrigin()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
m_data->m_rootLocalInertialFrame.getRotation()[0];
body->m_rootLocalInertialFrame.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
m_data->m_rootLocalInertialFrame.getRotation()[1];
body->m_rootLocalInertialFrame.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
m_data->m_rootLocalInertialFrame.getRotation()[2];
body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
m_data->m_rootLocalInertialFrame.getRotation()[3];
body->m_rootLocalInertialFrame.getRotation()[3];
@@ -1067,10 +1246,12 @@ void PhysicsServerSharedMemory::processClientCommands()
{
b3Printf("Server Init Pose not implemented yet");
}
int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
if (m_data->m_dynamicsWorld->getNumMultibodies()>body_unique_id)
int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(body_unique_id);
btMultiBody* mb = body->m_multiBody;
mb->setBasePos(btVector3(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1],

2
examples/SharedMemory/PhysicsServer.h
View File

@@ -25,7 +25,7 @@ protected:
void releaseSharedMemory();
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase);
bool useMultiBody, bool useFixedBase, int* bodyUniqueId);
public:
PhysicsServerSharedMemory();

4
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -170,7 +170,9 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
int maxSteps = 10;
while (maxSteps-- && rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}