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

22
examples/CommonInterfaces/CommonParameterInterface.h
View File

@@ -33,6 +33,7 @@ struct SliderParams
}; };
typedef void (*ButtonParamChangedCallback) (int buttonId, bool buttonState, void* userPointer); typedef void (*ButtonParamChangedCallback) (int buttonId, bool buttonState, void* userPointer);
typedef void (*ComboBoxCallback) (int combobox, const char* item, void* userPointer);
struct ButtonParams struct ButtonParams
{ {
@@ -50,6 +51,26 @@ struct ButtonParams
} }
}; };
struct ComboBoxParams
{
int m_comboboxId;
int m_numItems;
const char** m_items;
int m_startItem;
ComboBoxCallback m_callback;
void* m_userPointer;
ComboBoxParams()
:m_comboboxId(-1),
m_numItems(0),
m_items(0),
m_startItem(0),
m_callback(0),
m_userPointer(0)
{
}
};
struct CommonParameterInterface struct CommonParameterInterface
{ {
@@ -57,6 +78,7 @@ struct CommonParameterInterface
virtual ~CommonParameterInterface() {} virtual ~CommonParameterInterface() {}
virtual void registerSliderFloatParameter(SliderParams& params)=0; virtual void registerSliderFloatParameter(SliderParams& params)=0;
virtual void registerButtonParameter(ButtonParams& params)=0; virtual void registerButtonParameter(ButtonParams& params)=0;
virtual void registerComboBox(ComboBoxParams& params)=0;
virtual void syncParameters()=0; virtual void syncParameters()=0;
virtual void removeAllParameters()=0; virtual void removeAllParameters()=0;

70
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

@@ -83,7 +83,7 @@ struct GwenParameters
{ {
b3AlignedObjectArray<MySliderEventHandler<btScalar>*> m_sliderEventHandlers; b3AlignedObjectArray<MySliderEventHandler<btScalar>*> m_sliderEventHandlers;
b3AlignedObjectArray<Gwen::Controls::HorizontalSlider*> m_sliders; b3AlignedObjectArray<Gwen::Controls::HorizontalSlider*> m_sliders;
b3AlignedObjectArray<Gwen::Controls::ComboBox*> m_comboBoxes;
b3AlignedObjectArray<Gwen::Controls::Button*> m_buttons; b3AlignedObjectArray<Gwen::Controls::Button*> m_buttons;
b3AlignedObjectArray<MyButtonEventHandler*> m_buttonEventHandlers; b3AlignedObjectArray<MyButtonEventHandler*> m_buttonEventHandlers;
b3AlignedObjectArray<Gwen::Controls::TextBox*> m_textLabels; b3AlignedObjectArray<Gwen::Controls::TextBox*> m_textLabels;
@@ -143,6 +143,58 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
m_gwenInternalData->m_curYposition+=22; m_gwenInternalData->m_curYposition+=22;
}
struct MyComboBoxHander2 :public Gwen::Event::Handler
{
GwenInternalData* m_data;
int m_buttonId;
ComboBoxCallback m_callback;
void* m_userPointer;
MyComboBoxHander2 (GwenInternalData* data, int buttonId,ComboBoxCallback callback, void* userPointer)
:m_data(data),
m_buttonId(buttonId),
m_callback(callback),
m_userPointer(userPointer)
{
}
void onSelect( Gwen::Controls::Base* pControl )
{
Gwen::Controls::ComboBox* but = (Gwen::Controls::ComboBox*) pControl;
Gwen::String str = Gwen::Utility::UnicodeToString( but->GetSelectedItem()->GetText());
if (m_callback)
(*m_callback)(m_buttonId,str.c_str(),m_userPointer);
}
};
void GwenParameterInterface::registerComboBox(ComboBoxParams& params)
{
Gwen::Controls::ComboBox* combobox = new Gwen::Controls::ComboBox(m_gwenInternalData->m_demoPage->GetPage());
m_paramInternalData->m_comboBoxes.push_back(combobox);
MyComboBoxHander2* handler = new MyComboBoxHander2(m_gwenInternalData, params.m_comboboxId,params.m_callback, params.m_userPointer);
m_gwenInternalData->m_handlers.push_back(handler);
combobox->onSelection.Add(handler,&MyComboBoxHander2::onSelect);
int ypos = m_gwenInternalData->m_curYposition;
m_gwenInternalData->m_curYposition+=22;
combobox->SetPos(10, ypos );
combobox->SetWidth( 100 );
//box->SetPos(120,130);
for (int i=0;i<params.m_numItems;i++)
{
Gwen::Controls::MenuItem* item = combobox->AddItem(Gwen::Utility::StringToUnicode(params.m_items[i]));
if (i==params.m_startItem)
combobox->OnItemSelected(item);
}
} }
void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
@@ -188,6 +240,7 @@ void GwenParameterInterface::syncParameters()
void GwenParameterInterface::removeAllParameters() void GwenParameterInterface::removeAllParameters()
{ {
for (int i=0;i<m_paramInternalData->m_buttons.size();i++) for (int i=0;i<m_paramInternalData->m_buttons.size();i++)
{ {
delete m_paramInternalData->m_buttons[i]; delete m_paramInternalData->m_buttons[i];
@@ -222,5 +275,18 @@ void GwenParameterInterface::removeAllParameters()
} }
m_paramInternalData->m_textLabels.clear(); m_paramInternalData->m_textLabels.clear();
for (int i=0;i<m_paramInternalData->m_comboBoxes.size();i++)
{
delete m_paramInternalData->m_comboBoxes[i];
}
m_paramInternalData->m_comboBoxes.clear();
m_gwenInternalData->m_curYposition = this->m_paramInternalData->m_savedYposition; m_gwenInternalData->m_curYposition = this->m_paramInternalData->m_savedYposition;
} for (int i=0;i<m_gwenInternalData->m_handlers.size();i++)
{
delete m_gwenInternalData->m_handlers[i];
}
m_gwenInternalData->m_handlers.clear();
}

3
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h
View File

@@ -13,10 +13,13 @@ struct GwenParameterInterface : public CommonParameterInterface
virtual ~GwenParameterInterface(); virtual ~GwenParameterInterface();
virtual void registerSliderFloatParameter(SliderParams& params); virtual void registerSliderFloatParameter(SliderParams& params);
virtual void registerButtonParameter(ButtonParams& params); virtual void registerButtonParameter(ButtonParams& params);
virtual void registerComboBox(ComboBoxParams& params);
virtual void setSliderValue(int sliderIndex, double sliderValue); virtual void setSliderValue(int sliderIndex, double sliderValue);
virtual void syncParameters(); virtual void syncParameters();
virtual void removeAllParameters(); virtual void removeAllParameters();
}; };

4
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@@ -428,7 +428,7 @@ void GwenUserInterface::setToggleButtonCallback(b3ToggleButtonCallback callback)
{ {
m_data->m_toggleButtonCallback = callback; m_data->m_toggleButtonCallback = callback;
} }
void GwenUserInterface::registerToggleButton(int buttonId, const char* name) void GwenUserInterface::registerToggleButton2(int buttonId, const char* name)
{ {
assert(m_data); assert(m_data);
assert(m_data->m_demoPage); assert(m_data->m_demoPage);
@@ -460,7 +460,7 @@ b3ComboBoxCallback GwenUserInterface::getComboBoxCallback()
{ {
return m_data->m_comboBoxCallback; return m_data->m_comboBoxCallback;
} }
void GwenUserInterface::registerComboBox(int comboboxId, int numItems, const char** items, int startItem) void GwenUserInterface::registerComboBox2(int comboboxId, int numItems, const char** items, int startItem)
{ {
Gwen::Controls::ComboBox* combobox = new Gwen::Controls::ComboBox(m_data->m_demoPage->GetPage()); Gwen::Controls::ComboBox* combobox = new Gwen::Controls::ComboBox(m_data->m_demoPage->GetPage());
MyComboBoxHander* handler = new MyComboBoxHander(m_data, comboboxId); MyComboBoxHander* handler = new MyComboBoxHander(m_data, comboboxId);

4
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
View File

@@ -41,11 +41,11 @@ class GwenUserInterface
void setToggleButtonCallback(b3ToggleButtonCallback callback); void setToggleButtonCallback(b3ToggleButtonCallback callback);
b3ToggleButtonCallback getToggleButtonCallback(); b3ToggleButtonCallback getToggleButtonCallback();
void registerToggleButton(int buttonId, const char* name); void registerToggleButton2(int buttonId, const char* name);
void setComboBoxCallback(b3ComboBoxCallback callback); void setComboBoxCallback(b3ComboBoxCallback callback);
b3ComboBoxCallback getComboBoxCallback(); b3ComboBoxCallback getComboBoxCallback();
void registerComboBox(int buttonId, int numItems, const char** items, int startItem = 0); void registerComboBox2(int buttonId, int numItems, const char** items, int startItem = 0);
void setStatusBarMessage(const char* message, bool isLeft=true); void setStatusBarMessage(const char* message, bool isLeft=true);

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -24,9 +24,9 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0; 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; virtual void setSharedMemoryKey(int key) = 0;

50
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -135,6 +135,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
b3Assert(command); b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE; command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
command->m_updateFlags = 0; command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
@@ -197,6 +198,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE; command->m_type =CMD_REQUEST_ACTUAL_STATE;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = 0;
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
@@ -204,12 +206,17 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status); b3Assert(status);
b3JointInfo info; int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3GetJointInfo(physClient, jointIndex, &info); b3Assert(bodyIndex>=0);
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; if (bodyIndex>=0)
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex]; {
for (int ii(0); ii < 6; ++ii) { b3JointInfo info;
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; 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_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0; command->m_createSensorArguments.m_numJointSensorChanges = 0;
command->m_createSensorArguments.m_bodyUniqueId = 0;
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
@@ -349,7 +357,29 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
return 0; 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) int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{ {
PhysicsClient* cl = (PhysicsClient* ) 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; 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; PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(linkIndex,*info); cl->getJointInfo(bodyIndex, linkIndex,*info);
} }
int b3PickBody(struct SharedMemoryCommand *command, 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 b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
int b3GetNumJoints(b3PhysicsClientHandle physClient);
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); b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);

150
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -29,7 +29,8 @@ protected:
bool m_wantsTermination; bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests; btAlignedObjectArray<int> m_userCommandRequests;
int m_sharedMemoryKey; int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
void createButton(const char* name, int id, bool isTrigger ); void createButton(const char* name, int id, bool isTrigger );
void createButtons(); void createButtons();
@@ -45,12 +46,22 @@ public:
virtual ~PhysicsClientExample(); virtual ~PhysicsClientExample();
virtual void initPhysics(); 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 stepSimulation(float deltaTime);
virtual void resetCamera() virtual void resetCamera()
{ {
float dist = 5; float dist = 5;
float pitch = 50; float pitch = 50;
float yaw = 35; float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5}; 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) void MyCallback(int buttonId, bool buttonState, void* userPtr)
{ {
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr; PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
if (buttonState) b3Assert(cl);
if (cl && buttonState)
{ {
cl->enqueueCommand(buttonId); cl->enqueueCommand(buttonId);
} }
@@ -173,9 +198,10 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
//setting the initial position, orientation and other arguments are optional //setting the initial position, orientation and other arguments are optional
double startPosX = 0; double startPosX = 0;
double startPosY = 0; static double startPosY = 0;
double startPosZ = 0; double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
@@ -252,6 +278,11 @@ m_numMotors(0)
PhysicsClientExample::~PhysicsClientExample() PhysicsClientExample::~PhysicsClientExample()
{ {
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
b3Printf("~PhysicsClientExample\n"); b3Printf("~PhysicsClientExample\n");
} }
@@ -269,6 +300,8 @@ void PhysicsClientExample::createButtons()
if (m_guiHelper && m_guiHelper->getParameterInterface()) if (m_guiHelper && m_guiHelper->getParameterInterface())
{ {
m_guiHelper->getParameterInterface()->removeAllParameters();
createButton("Load URDF",CMD_LOAD_URDF, isTrigger); createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, 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("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,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() void PhysicsClientExample::initPhysics()
{ {
if (m_guiHelper && m_guiHelper->getParameterInterface()) if (m_guiHelper && m_guiHelper->getParameterInterface())
@@ -296,6 +364,9 @@ void PhysicsClientExample::initPhysics()
MyCallback(CMD_RESET_SIMULATION,true,this); MyCallback(CMD_RESET_SIMULATION,true,this);
} }
m_selectedBody = -1;
m_prevSelectedBody = -1;
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
if (!b3CanSubmitCommand(m_physicsClientHandle)) if (!b3CanSubmitCommand(m_physicsClientHandle))
{ {
@@ -307,6 +378,11 @@ void PhysicsClientExample::initPhysics()
void PhysicsClientExample::stepSimulation(float deltaTime) void PhysicsClientExample::stepSimulation(float deltaTime)
{ {
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0); bool hasStatus = (status != 0);
@@ -317,43 +393,34 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (statusType == CMD_URDF_LOADING_COMPLETED) if (statusType == CMD_URDF_LOADING_COMPLETED)
{ {
int numJoints = b3GetNumJoints(m_physicsClientHandle); int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
for (int i=0;i<numJoints;i++)
{ {
b3JointInfo info; int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
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); 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 //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (commandId ==CMD_RESET_SIMULATION) if (commandId ==CMD_RESET_SIMULATION)
{ {
if (m_guiHelper->getParameterInterface()) m_selectedBody = -1;
{
m_guiHelper->getParameterInterface()->removeAllParameters();
}
m_numMotors=0; m_numMotors=0;
createButtons(); createButtons();
} }

59
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -30,12 +30,17 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z) {
return tmp; return tmp;
} }
struct BodyJointInfoCache
{
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
struct PhysicsClientSharedMemoryInternalData { struct PhysicsClientSharedMemoryInternalData {
SharedMemoryInterface* m_sharedMemory; SharedMemoryInterface* m_sharedMemory;
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom; btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo; btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor; btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
@@ -66,10 +71,29 @@ struct PhysicsClientSharedMemoryInternalData {
bool canSubmitCommand() const; 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 { return bodyJoints->m_jointInfo.size();
info = m_data->m_jointInfo[index]; }
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() PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -169,7 +193,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
serverCmd.m_dataStreamArguments.m_streamChunkLength); serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf->setFileDNAisMemoryDNA(); bf->setFileDNAisMemoryDNA();
bf->parse(false); 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++) { for (int i = 0; i < bf->m_multiBodies.size(); i++) {
int flag = bf->getFlags(); int flag = bf->getFlags();
@@ -214,7 +241,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
(mb->m_links[link].m_jointType == ePrismaticType)) { (mb->m_links[link].m_jointType == ePrismaticType)) {
info.m_flags |= JOINT_HAS_MOTORIZED_POWER; 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; qOffset += mb->m_links[link].m_posVarCount;
uOffset += mb->m_links[link].m_dofCount; uOffset += mb->m_links[link].m_dofCount;
@@ -257,7 +284,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
(mb->m_links[link].m_jointType == ePrismaticType)) { (mb->m_links[link].m_jointType == ePrismaticType)) {
info.m_flags |= JOINT_HAS_MOTORIZED_POWER; 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; qOffset += mb->m_links[link].m_posVarCount;
uOffset += mb->m_links[link].m_dofCount; uOffset += mb->m_links[link].m_dofCount;
@@ -367,12 +394,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) { if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
} }
for (int i = 0; i < m_data->m_robotMultiBodyData.size(); i++) { for (int i=0;i<m_data->m_bodyJointMap.size();i++)
delete m_data->m_robotMultiBodyData[i]; {
} BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
m_data->m_robotMultiBodyData.clear(); if (bodyJointsPtr && *bodyJointsPtr)
{
m_data->m_jointInfo.clear(); delete (*bodyJointsPtr);
}
}
m_data->m_bodyJointMap.clear();
break; break;
} }
case CMD_DEBUG_LINES_COMPLETED: { 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 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); 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); 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 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; SharedMemoryInterface* m_sharedMemory;
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
bool m_isConnected; bool m_isConnected;
btScalar m_physicsDeltaTime; btScalar m_physicsDeltaTime;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks; btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters; btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper; btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap; btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
@@ -95,7 +217,7 @@ struct PhysicsServerInternalData
btMultiBodyDynamicsWorld* m_dynamicsWorld; btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_debugDrawer; SharedMemoryDebugDrawer* m_debugDrawer;
btTransform m_rootLocalInertialFrame;
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
@@ -127,7 +249,51 @@ struct PhysicsServerInternalData
m_pickedConstraint(0), m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(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) 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 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); btAssert(m_data->m_dynamicsWorld);
if (!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); bool loadOk = u2b.loadURDF(fileName, useFixedBase);
if (loadOk) if (loadOk)
{ {
//get a body index
int bodyUniqueId = m_data->allocHandle();
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
{ {
btScalar mass = 0; btScalar mass = 0;
m_data->m_rootLocalInertialFrame.setIdentity(); bodyHandle->m_rootLocalInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0,0,0); btVector3 localInertiaDiagonal(0,0,0);
int urdfLinkIndex = u2b.getRootLinkIndex(); 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) if (m_data->m_verboseOutput)
{ {
@@ -463,6 +636,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
{ {
if (mb) if (mb)
{ {
bodyHandle->m_multiBody = mb;
createJointMotors(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 //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: 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; const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
if (m_data->m_verboseOutput) 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 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; 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 //load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn, initialPos,initialOrn,
useMultiBody, useFixedBase); useMultiBody, useFixedBase,&bodyUniqueId);
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
if (completedOk) 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); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); 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); m_data->submitServerStatus(status);
} else } else
@@ -701,10 +871,11 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
b3Printf("Processed CMD_CREATE_SENSOR"); b3Printf("Processed CMD_CREATE_SENSOR");
} }
int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId;
if (m_data->m_dynamicsWorld->getNumMultibodies()>0) 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); btAssert(mb);
for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++) for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++)
{ {
@@ -767,9 +938,13 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
b3Printf("Processed CMD_SEND_DESIRED_STATE"); 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); btAssert(mb);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
@@ -813,9 +988,7 @@ void PhysicsServerSharedMemory::processClientCommands()
int numMotors = 0; int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque //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 int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link=0;link<mb->getNumLinks();link++) for (int link=0;link<mb->getNumLinks();link++)
{ {
@@ -851,9 +1024,7 @@ void PhysicsServerSharedMemory::processClientCommands()
int numMotors = 0; int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque //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 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 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++) for (int link=0;link<mb->getNumLinks();link++)
@@ -916,15 +1087,23 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
b3Printf("Sending the actual state (Q,U)"); 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); SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0; int totalDegreeOfFreedomU = 0;
//always add the base, even for static (non-moving objects) //always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed //so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())" //do we don't use this conditional "if (!mb->hasFixedBase())"
@@ -934,20 +1113,20 @@ void PhysicsServerSharedMemory::processClientCommands()
tr.setRotation(mb->getWorldToBaseRot().inverse()); tr.setRotation(mb->getWorldToBaseRot().inverse());
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
m_data->m_rootLocalInertialFrame.getOrigin()[0]; body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
m_data->m_rootLocalInertialFrame.getOrigin()[1]; body->m_rootLocalInertialFrame.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
m_data->m_rootLocalInertialFrame.getOrigin()[2]; body->m_rootLocalInertialFrame.getOrigin()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
m_data->m_rootLocalInertialFrame.getRotation()[0]; body->m_rootLocalInertialFrame.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
m_data->m_rootLocalInertialFrame.getRotation()[1]; body->m_rootLocalInertialFrame.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
m_data->m_rootLocalInertialFrame.getRotation()[2]; body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 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"); b3Printf("Server Init Pose not implemented yet");
} }
int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
if (m_data->m_dynamicsWorld->getNumMultibodies()>body_unique_id) 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( mb->setBasePos(btVector3(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0], clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1], clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1],

2
examples/SharedMemory/PhysicsServer.h
View File

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

4
examples/SharedMemory/PhysicsServerExample.cpp
View File

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

132
test/SharedMemory/test.c
View File

@@ -1,5 +1,6 @@
//#include "SharedMemoryCommands.h" //#include "SharedMemoryCommands.h"
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Logging.h"
#include <string.h> #include <string.h>
@@ -24,7 +25,8 @@ int main(int argc, char* argv[])
b3PhysicsClientHandle sm; b3PhysicsClientHandle sm;
int bodyIndex = -1;
printf("hello world\n"); printf("hello world\n");
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
@@ -37,62 +39,68 @@ int main(int argc, char* argv[])
b3SubmitClientCommandAndWaitStatus(sm, command); b3SubmitClientCommandAndWaitStatus(sm, command);
} }
{ {
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional //setting the initial position, orientation and other arguments are optional
startPosX =2; startPosX =2;
startPosY =3; startPosY =3;
startPosZ = 1; startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
b3SubmitClientCommandAndWaitStatus(sm, command);
}
numJoints = b3GetNumJoints(sm);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,i,&jointInfo);
printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
bodyIndex = b3GetStatusBodyIndex(statusHandle);
} }
if (bodyIndex>=0)
{
numJoints = b3GetNumJoints(sm,bodyIndex);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
}
{ {
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
@@ -124,6 +132,7 @@ int main(int argc, char* argv[])
} }
{ {
#if 0
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++) for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
@@ -132,7 +141,7 @@ int main(int argc, char* argv[])
b3JointControlSetMaximumForce(command,dofIndex,100); b3JointControlSetMaximumForce(command,dofIndex,100);
} }
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
} }
///perform some simulation steps for testing ///perform some simulation steps for testing
for ( i=0;i<100;i++) for ( i=0;i<100;i++)
@@ -145,23 +154,32 @@ int main(int argc, char* argv[])
} }
#if 0
if (sensorJointIndexLeft>=0) if (sensorJointIndexLeft>=0)
{ {
struct b3JointSensorState info;
b3GetJointInfo(sm,bodyIndex,sensorJointIndexLeft,&info);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0], info.m_jointForceTorque[0],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1], info.m_jointForceTorque[1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]); info.m_jointForceTorque[2]);
} }
if (sensorJointIndexRight>=0) if (sensorJointIndexRight>=0)
{ {
struct b3JointSensorState info;
b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&info);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0], info.m_jointForceTorque[0],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1], info.m_jointForceTorque[1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]); info.m_jointForceTorque[2]);
} }
#endif
{ {
b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
} }