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

another step closer to useable shared memory C API

Browse Source 
(force/torque sensor needs new API)
in a nutshell, users of shared memory physics API should not
directly poke into shared memory, not fill 'SharedMemorCommand'
nor read SharedMemoryStatus directly. The C-API declares 'handles' for those,
to avoid it from happening.
This commit is contained in:
=
2015-09-16 23:09:10 -07:00
parent 4041748f55
commit 003a42478b
16 changed files with 671 additions and 489 deletions
Download Patch File Download Diff File Expand all files Collapse all files

418
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -7,7 +7,9 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsClient.h"
#include "SharedMemoryCommands.h"
//#include "SharedMemoryCommands.h"
#include "PhysicsClientC_API.h"
struct MyMotorInfo2
{
@@ -16,17 +18,18 @@ struct MyMotorInfo2
int m_uIndex;
};
#define MAX_NUM_MOTORS 128
class PhysicsClientExample : public SharedMemoryCommon
{
protected:
PhysicsClientSharedMemory m_physicsClient;
b3PhysicsClientHandle m_physicsClientHandle;
bool m_wantsTermination;
btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
btAlignedObjectArray<int> m_userCommandRequests;
int m_sharedMemoryKey;
void createButton(const char* name, int id, bool isTrigger );
void createButtons();
@@ -62,18 +65,22 @@ public:
virtual bool isConnected()
{
return m_physicsClient.isConnected();
return (m_physicsClientHandle!=0);
}
void enqueueCommand(const SharedMemoryCommand& orgCommand);
void enqueueCommand(int commandId);
void prepareAndSubmitCommand(int commandId);
virtual void exitPhysics(){};
virtual void renderScene()
{
int numLines = m_physicsClient.getNumDebugLines();
const btVector3* fromLines = m_physicsClient.getDebugLinesFrom();
const btVector3* toLines = m_physicsClient.getDebugLinesTo();
const btVector3* colorLines = m_physicsClient.getDebugLinesColor();
b3DebugLines debugLines;
b3GetDebugLines(m_physicsClientHandle,&debugLines);
int numLines = debugLines.m_numDebugLines;
int lineWidth = 1;
@@ -86,12 +93,12 @@ public:
for (int i=0;i<numLines;i++)
{
points[i*2].m_floats[0] = fromLines[i].x();
points[i*2].m_floats[1] = fromLines[i].y();
points[i*2].m_floats[2] = fromLines[i].z();
points[i*2+1].m_floats[0] = toLines[i].x();
points[i*2+1].m_floats[1] = toLines[i].y();
points[i*2+1].m_floats[2] = toLines[i].z();
points[i*2].m_floats[0] = debugLines.m_linesFrom[i*3+0];
points[i*2].m_floats[1] = debugLines.m_linesFrom[i*3+1];
points[i*2].m_floats[2] = debugLines.m_linesFrom[i*3+2];
points[i*2+1].m_floats[0] = debugLines.m_linesTo[i*3+0];
points[i*2+1].m_floats[1] = debugLines.m_linesTo[i*3+1];
points[i*2+1].m_floats[2] = debugLines.m_linesTo[i*3+2];
indices[i*2] = i*2;
indices[i*2+1] = i*2+1;
}
@@ -108,32 +115,21 @@ public:
{
for (int i=0;i<numLines;i++)
{
m_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth);
m_guiHelper->getRenderInterface()->drawLine(debugLines.m_linesFrom,debugLines.m_linesTo,debugLines.m_linesColor,lineWidth);
}
}
}
void prepareControlCommand(SharedMemoryCommand& command)
void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
{
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<m_numMotors;i++)
{
btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
int uIndex = m_motorTargetVelocities[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
}
}
virtual void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;};
@@ -143,7 +139,7 @@ public:
virtual void setSharedMemoryKey(int key)
{
m_physicsClient.setSharedMemoryKey(key);
m_sharedMemoryKey = key;
}
};
@@ -154,102 +150,103 @@ public:
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
if (buttonState)
{
cl->enqueueCommand(buttonId);
}
SharedMemoryCommand command;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");
command.m_urdfArguments.m_initialPosition[0] = 0.0;
command.m_updateFlags =
URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE;
command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
command.m_urdfArguments.m_useFixedBase = false;
command.m_urdfArguments.m_useMultiBody = true;
cl->enqueueCommand(command);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
cl->enqueueCommand(command);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
command.m_type =CMD_REQUEST_ACTUAL_STATE;
cl->enqueueCommand(command);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command);
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//:DBG_DrawConstraints;
command.m_requestDebugLinesArguments.m_startingLineIndex = 0;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_DESIRED_STATE:
{
command.m_type =CMD_SEND_DESIRED_STATE;
cl->prepareControlCommand(command);
cl->enqueueCommand(command);
break;
}
case CMD_RESET_SIMULATION:
{
command.m_type = CMD_RESET_SIMULATION;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
command.m_type = buttonId;
cl->enqueueCommand(command);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
void PhysicsClientExample::enqueueCommand(int commandId)
{
m_userCommandRequests.push_back(commandId);
}
//b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
switch (commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "r2d2.urdf");//kuka_lwr/kuka.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
double startPosY = 0;
double startPosZ = 0;
int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_DEBUG_LINES:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_DESIRED_STATE:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
break;
}
case CMD_RESET_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
#if 0
//this worked, but needs C-API and a streaming options, similar to debug lines
command.m_type = buttonId;
cl->enqueueCommand(command);
#endif
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsTermination(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_numMotors(0)
{
b3Printf("Started PhysicsClientExample\n");
@@ -299,27 +296,11 @@ void PhysicsClientExample::initPhysics()
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_RESET_SIMULATION,true,this);
// MyCallback(CMD_LOAD_URDF, true, this);
// MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
// MyCallback(CMD_RESET_SIMULATION,true,this);
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
}
if (!m_physicsClient.connect())
{
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
}
@@ -328,113 +309,96 @@ void PhysicsClientExample::initPhysics()
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_physicsClient.isConnected())
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
int statusType = b3GetStatusType(status);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
int numJoints = b3GetNumJoints(m_physicsClientHandle);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
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 (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
for (int i=0;i<numJoints;i++)
{
for (int i=0;i<m_physicsClient.getNumJoints();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)
{
b3JointInfo info;
m_physicsClient.getJointInfo(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)
{
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())
{
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++;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
}
}
}
}
if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED)
{
SharedMemoryCommand command;
if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)
{
//continue requesting debug lines for drawing
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints;
command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex;
enqueueCommand(command);
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
SharedMemoryCommand command = m_userCommandRequests[0];
}
if (b3CanSubmitCommand(m_physicsClientHandle))
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int commandId = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (command.m_type==CMD_RESET_SIMULATION)
{
if (m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->removeAllParameters();
}
m_numMotors=0;
createButtons();
}
m_physicsClient.submitClientCommand(command);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
m_userCommandRequests.pop_back();
//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_numMotors=0;
createButtons();
}
prepareAndSubmitCommand(commandId);
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
}
}
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
command.m_type = CMD_REQUEST_ACTUAL_STATE;
enqueueCommand(command);
}
}
}
}
}