Merge pull request #838 from erwincoumans/master

first version of 'getVisualShapeData' for pybullet / shmem API
This commit is contained in:
erwincoumans 2016-10-19 10:50:56 -07:00 committed by GitHub
commit ad07b3cf13
25 changed files with 512 additions and 35 deletions

View File

@ -1,8 +1,21 @@
IF NOT EXIST bin mkdir bin
IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin
#optionally, hardcode the python path or
#SET myvar=c:\python-3.5.3
#find a python version (hopefully just 1) and use this
dir c:\python* /b /ad > tmp1234.txt
set /p myvar1= < tmp1234.txt
set myvar=c:/%myvar1%
del tmp1234.txt
#you can also override and hardcode the Python path like this (just remove the # hashmark in next line)
#SET myvar=c:\python-3.5.2
cd build3
premake4 --double --enable_openvr --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --targetdir="../bin" vs2010
premake4 --double --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
start vs2010

Binary file not shown.

View File

@ -1211,10 +1211,6 @@ void OpenGLExampleBrowser::update(float deltaTime)
{
m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
}
if (gui2)
{
gui2->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
}
if (sUseOpenGL2)
{

View File

@ -228,7 +228,7 @@ void ImportSDFSetup::initPhysics()
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);

View File

@ -1144,13 +1144,13 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
return false;
}
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
{
if (m_data->m_customVisualShapesConverter)
{
const UrdfModel& model = m_data->m_urdfParser.getModel();
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId);
}
}

View File

@ -51,7 +51,7 @@ public:
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation

View File

@ -238,7 +238,7 @@ void ImportUrdfSetup::initPhysics()
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());

View File

@ -136,8 +136,8 @@ void MultiDofDemo::initPhysics()
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = false;
bool canSleep = true;
bool selfCollide = false;
bool multibodyConstraint = true;
bool selfCollide = true;
bool multibodyConstraint = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
@ -159,6 +159,7 @@ void MultiDofDemo::initPhysics()
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0));
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
//////////////////////////////////////////////
if(numLinks > 0)
{
@ -247,8 +248,8 @@ void MultiDofDemo::initPhysics()
frameInA.setIdentity();
frameInB.setIdentity();
btVector3 jointAxis(1.0,0.0,0.0);
btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
//btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
//btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
p2p->setMaxAppliedImpulse(2.0);
m_dynamicsWorld->addMultiBodyConstraint(p2p);
}
@ -306,10 +307,10 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
for(int i = 0; i < numLinks; ++i)
{
if(!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();

View File

@ -48,6 +48,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
};
#endif // BT_PHYSICS_CLIENT_API_H

View File

@ -1227,6 +1227,33 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
}
//request visual shape information
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
command->m_requestVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA;
command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedVisualShapeInformation(visualShapeInfo);
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

View File

@ -93,7 +93,12 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
//request visual shape information
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);

View File

@ -25,6 +25,11 @@ struct MyMotorInfo2
int camVisualizerWidth = 320;//1024/3;
int camVisualizerHeight = 240;//768/3;
enum CustomCommands
{
CMD_CUSTOM_SET_REALTIME_SIMULATION = CMD_MAX_CLIENT_COMMANDS+1,
CMD_CUSTOM_SET_GRAVITY
};
#define MAX_NUM_MOTORS 128
@ -306,7 +311,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
for (int i = 0; i < numJoints; ++i) {
struct b3JointSensorState sensorState;
b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
//b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
}
}
break;
@ -410,10 +415,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
case CMD_CUSTOM_SET_GRAVITY: {
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CUSTOM_SET_REALTIME_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetRealTimeSimulation(commandHandle,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
@ -458,6 +471,16 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
if (m_selectedBody >= 0)
{
//request visual shape information
b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
}
default:
{
b3Error("Unknown buttonId");
@ -535,6 +558,8 @@ void PhysicsClientExample::createButtons()
createButton("Save World",CMD_SAVE_WORLD, isTrigger);
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger);
createButton("Get Visual Shape Info",CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
if (m_options!=eCLIENTEXAMPLE_SERVER)
{
@ -545,7 +570,7 @@ void PhysicsClientExample::createButtons()
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
createButton("Set gravity", CMD_CUSTOM_SET_GRAVITY, isTrigger);
createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger);
@ -584,7 +609,7 @@ void PhysicsClientExample::createButtons()
{
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);
//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)
{
@ -959,7 +984,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
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);
//b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}
@ -968,6 +993,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
b3Warning("Cannot get contact information");
}
if (statusType == CMD_VISUAL_SHAPE_INFO_FAILED)
{
b3Warning("Cannot get visual shape information");
}
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
b3VisualShapeInformation shapeInfo;
b3GetVisualShapeInformation(m_physicsClientHandle, &shapeInfo);
b3Printf("Num visual shapes: %d", shapeInfo.m_numVisualShapes);
}
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
b3ContactInformation contactPointData;

View File

@ -41,6 +41,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@ -633,7 +635,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Calculate Inverse Kinematics Request failed");
break;
}
case CMD_VISUAL_SHAPE_INFO_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Visual Shape Information Request OK\n");
}
int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex;
int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied);
b3VisualShapeData* shapeData = (b3VisualShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numVisualShapesCopied; i++)
{
m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i];
}
break;
}
case CMD_VISUAL_SHAPE_INFO_FAILED:
{
b3Warning("Visual Shape Info Request failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@ -710,6 +733,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
{
command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@ -813,6 +851,13 @@ void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3Contac
}
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {
return &m_data->m_debugLinesFrom[0].m_x;

View File

@ -56,6 +56,8 @@ public:
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};
#endif // BT_PHYSICS_CLIENT_API_H

View File

@ -42,7 +42,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
PhysicsServerCommandProcessor* m_commandProcessor;
@ -196,6 +196,44 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
{
b3Printf("Visual Shape Information Request OK\n");
}
int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex;
int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied);
b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i = 0; i < numVisualShapesCopied; i++)
{
m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i];
}
if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
{
command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId;
}
}
} while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied);
return m_data->m_hasStatus;
}
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
@ -380,6 +418,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return processContactPointData(command);
}
if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO)
{
return processVisualShapeData(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
@ -576,3 +619,8 @@ void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation
}
void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}

View File

@ -23,6 +23,8 @@ protected:
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
@ -74,6 +76,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();

View File

@ -140,3 +140,8 @@ void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformati
{
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
}
void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
}

View File

@ -62,6 +62,7 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};
#endif //PHYSICS_LOOP_BACK_H

View File

@ -2971,7 +2971,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED;
//retrieve the visual shape information for a specific body
int totalNumVisualShapes = m_data->m_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId);
int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
shapeIndex,
visualShapeStoragePtr);
//m_visualConverter
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");

View File

@ -151,6 +151,21 @@ struct RequestContactDataArgs
int m_objectBIndexFilter;
};
struct RequestVisualShapeDataArgs
{
int m_bodyUniqueId;
int m_startingVisualShapeIndex;
};
struct SendVisualShapeDataArgs
{
int m_bodyUniqueId;
int m_startingVisualShapeIndex;
int m_numVisualShapesCopied;
int m_numRemainingVisualShapes;
};
struct SendDebugLinesArgs
{
@ -461,6 +476,7 @@ struct SharedMemoryCommand
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
};
};
@ -496,6 +512,7 @@ struct SharedMemoryStatus
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
};
};

View File

@ -34,6 +34,7 @@ enum EnumSharedMemoryClientCommand
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
CMD_SAVE_WORLD,
CMD_REQUEST_VISUAL_SHAPE_INFO,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@ -78,7 +79,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_SAVE_WORLD_COMPLETED,
CMD_SAVE_WORLD_FAILED,
CMD_VISUAL_SHAPE_INFO_COMPLETED,
CMD_VISUAL_SHAPE_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@ -188,6 +190,25 @@ struct b3ContactInformation
struct b3ContactPointData* m_contactPointData;
};
#define VISUAL_SHAPE_MAX_PATH_LEN 128
struct b3VisualShapeData
{
int m_objectUniqueId;
int m_linkIndex;
int m_visualGeometryType;//box primitive, sphere primitive, triangle mesh
double m_dimensions[3];//meaning depends on m_visualGeometryType
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
double m_localInertiaFrame[7];//pos[3], orn[4]
//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
};
struct b3VisualShapeInformation
{
int m_numVisualShapes;
struct b3VisualShapeData* m_visualShapeData;
};
///b3LinkState provides extra information such as the Cartesian world coordinates
///center of mass (COM) of the link, relative to the world reference frame.
///Orientation is a quaternion x,y,z,w

View File

@ -32,6 +32,7 @@ subject to the following restrictions:
#include <iostream>
#include <fstream>
#include "../Importers/ImportURDFDemo/UrdfParser.h"
#include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData
enum MyFileType
@ -61,6 +62,8 @@ struct TinyRendererVisualShapeConverterInternalData
btHashMap<btHashPtr,TinyRendererObjectArray*> m_swRenderInstances;
btAlignedObjectArray<b3VisualShapeData> m_visualShapes;
int m_upAxis;
int m_swWidth;
int m_swHeight;
@ -105,9 +108,14 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
{
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
visualShapeOut.m_dimensions[0] = 0;
visualShapeOut.m_dimensions[1] = 0;
visualShapeOut.m_dimensions[2] = 0;
visualShapeOut.m_meshAssetFileName[0] = 0;
GLInstanceGraphicsShape* glmesh = 0;
@ -119,6 +127,9 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
{
btAlignedObjectArray<btVector3> vertices;
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
@ -140,6 +151,9 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
}
case URDF_GEOM_BOX:
{
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0];
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_boxSize[1];
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_boxSize[2];
btVector3 extents = visual->m_geometry.m_boxSize;
@ -151,6 +165,8 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
}
case URDF_GEOM_SPHERE:
{
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius;
btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
@ -200,6 +216,18 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0];
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
int sl = strlen(fullPath);
if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1))
{
memcpy(visualShapeOut.m_meshAssetFileName, fullPath, sl);
visualShapeOut.m_meshAssetFileName[sl] = 0;
}
FILE* f = fopen(fullPath, "rb");
if (f)
{
@ -443,7 +471,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex)
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
{
@ -486,11 +514,23 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
btAssert(visualsPtr);
TinyRendererObjectArray* visuals = *visualsPtr;
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
b3VisualShapeData visualShape;
visualShape.m_objectUniqueId = bodyUniqueId;
visualShape.m_linkIndex = linkIndex;
visualShape.m_localInertiaFrame[0] = localInertiaFrame.getOrigin()[0];
visualShape.m_localInertiaFrame[1] = localInertiaFrame.getOrigin()[1];
visualShape.m_localInertiaFrame[2] = localInertiaFrame.getOrigin()[2];
visualShape.m_localInertiaFrame[3] = localInertiaFrame.getRotation()[0];
visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1];
visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2];
visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3];
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
m_data->m_visualShapes.push_back(visualShape);
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
unsigned char* textureImage=0;
int textureWidth=0;
int textureHeight=0;
@ -513,6 +553,64 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
}
}
int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)
{
int start = -1;
//find first one, then count how many
for (int i = 0; i < m_data->m_visualShapes.size(); i++)
{
if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId)
{
start = i;
break;
}
}
int count = 0;
if (start >= 0)
{
for (int i = start; i < m_data->m_visualShapes.size(); i++)
{
if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId)
{
count++;
}
else
{
//storage of each visual shape for a given body unique id assumed to be contiguous
break;
}
}
}
return count;
}
int TinyRendererVisualShapeConverter::getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData)
{
int start = -1;
//find first one, then count how many
for (int i = 0; i < m_data->m_visualShapes.size(); i++)
{
if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId)
{
start = i;
break;
}
}
int count = 0;
if (start >= 0)
{
if (start + shapeIndex < m_data->m_visualShapes.size())
{
*shapeData = m_data->m_visualShapes[start + shapeIndex];
return 1;
}
}
return 0;
}
void TinyRendererVisualShapeConverter::setUpAxis(int axis)
{
m_data->m_upAxis = axis;

View File

@ -4,6 +4,9 @@
#include "../Importers/ImportURDFDemo/LinkVisualShapesConverter.h"
struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
{
@ -15,6 +18,10 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
virtual int getNumVisualShapes(int bodyUniqueId);
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
void setUpAxis(int axis);
void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);

View File

@ -1260,6 +1260,107 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
return 0;
}
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
{
int size = PySequence_Size(args);
int objectUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3VisualShapeInformation visualShapeInfo;
int statusType;
int i;
PyObject* pyResultList = 0;
if (size == 1)
{
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
b3GetVisualShapeInformation(sm, &visualShapeInfo);
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
{
PyObject* visualShapeObList = PyTuple_New(7);
PyObject* item;
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
PyTuple_SetItem(visualShapeObList, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_linkIndex);
PyTuple_SetItem(visualShapeObList, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_visualGeometryType);
PyTuple_SetItem(visualShapeObList, 2, item);
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 3, vec);
}
item = PyString_FromString(visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName);
PyTuple_SetItem(visualShapeObList, 4, item);
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 5, vec);
}
{
PyObject* vec = PyTuple_New(4);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[3]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[4]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[5]);
PyTuple_SetItem(vec, 2, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[6]);
PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(visualShapeObList, 6, vec);
}
PyTuple_SetItem(pyResultList, i, visualShapeObList);
}
return pyResultList;
}
else
{
PyErr_SetString(SpamError, "Error receiving visual shape info");
return NULL;
}
}
else
{
PyErr_SetString(SpamError, "getVisualShapeData requires 1 argument (object unique id)");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
int size = PySequence_Size(args);
int objectUniqueIdA = -1;
@ -1287,7 +1388,6 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
commandHandle = b3InitRequestContactPointInformation(sm);
b3SetContactFilterBodyA(commandHandle, objectUniqueIdA);
b3SetContactFilterBodyB(commandHandle, objectUniqueIdB);
b3SubmitClientCommand(sm, commandHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@ -2194,6 +2294,9 @@ static PyMethodDef SpamMethods[] = {
"object-object collisions. Optional arguments one or two object unique "
"ids, that need to be involved in the contact."},
{ "getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
"Return the visual shape information for one object." },
{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
"Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to "
"quaternion [x,y,z,w]"},

View File

@ -67,6 +67,20 @@ void testSharedMemory(b3PhysicsClientHandle sm)
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
bodyUniqueId = bodyIndicesOut[0];
{
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
struct b3VisualShapeInformation vi;
b3GetVisualShapeInformation(sm, &vi);
}
}
}
numJoints = b3GetNumJoints(sm,bodyUniqueId);
ASSERT_EQ(numJoints,7);
@ -119,6 +133,8 @@ void testSharedMemory(b3PhysicsClientHandle sm)
if (bodyIndex>=0)
{
numJoints = b3GetNumJoints(sm,bodyIndex);
for (i=0;i<numJoints;i++)
{