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

PyBullet createVisualShape/createVisualShapeArray: postpone visual shape conversion until we know the link inertial transform

Browse Source
This commit is contained in:
erwincoumans
2018-03-28 19:08:18 -07:00
parent 6856334d48
commit ec68290497
1 changed files with 100 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -150,16 +150,16 @@ struct InternalVisualShapeData
int m_tinyRendererVisualShapeIndex;
int m_OpenGLGraphicsIndex;
UrdfVisual m_visualShape;
btTransform m_localInertiaFrame;
std::string m_pathPrefix;
b3AlignedObjectArray<UrdfVisual> m_visualShapes;
b3AlignedObjectArray<std::string> m_pathPrefixes;
void clear()
{
m_tinyRendererVisualShapeIndex = 0;
m_OpenGLGraphicsIndex = 0;
m_localInertiaFrame.setIdentity();
m_pathPrefix = "";
m_tinyRendererVisualShapeIndex = -1;
m_OpenGLGraphicsIndex = -1;
m_visualShapes.clear();
m_pathPrefixes.clear();
}
};
@@ -1921,10 +1921,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
if (visHandle->m_visualShape.m_geometry.m_hasLocalMaterial)
for (int i=0;i<visHandle->m_visualShapes.size();i++)
{
matCol = visHandle->m_visualShape.m_geometry.m_localMaterial.m_matColor;
return true;
if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
{
matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
return true;
}
}
}
}
@@ -2092,39 +2095,91 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
b3Assert(0);
}
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
int graphicsIndex = -1;
double globalScaling = 1.f;//todo!
int flags=0;
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<BulletURDFTexture> textures;
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
return visHandle->m_OpenGLGraphicsIndex;
if (visHandle->m_OpenGLGraphicsIndex>=0)
{
//instancing. assume the inertial frame is identical
graphicsIndex = visHandle->m_OpenGLGraphicsIndex;
} else
{
for (int v = 0;v<visHandle->m_visualShapes.size();v++)
{
u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse()*visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures);
}
if (vertices.size() && indices.size())
{
if (1)
{
int textureIndex = -1;
if (textures.size())
{
textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
}
{
B3_PROFILE("registerGraphicsShape");
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
visHandle->m_OpenGLGraphicsIndex = graphicsIndex;
}
}
}
}
}
}
return -1;
return graphicsIndex;
}
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
{
//if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
UrdfModel model;// = m_data->m_urdfParser.getModel();
UrdfLink link;
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
if (colShapeUniqueId>=0)
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
for (int i=0;i<visHandle->m_visualShapes.size();i++)
{
link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
link.m_visualArray.push_back(visHandle->m_visualShapes[i]);
}
}
}
if (link.m_visualArray.size()==0)
{
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
if (colShapeUniqueId>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
{
for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
{
link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
}
}
}
}
@@ -3968,19 +4023,16 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btTransform localInertiaFrame;
localInertiaFrame.setIdentity();
btTransform childTrans;
childTrans.setIdentity();
const char* pathPrefix = "";
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<BulletURDFTexture> textures;
int visualShapeUniqueId = -1;
UrdfVisual visualShape;
for (int userShapeIndex = 0; userShapeIndex< clientCmd.m_createUserShapeArgs.m_numUserShapes; userShapeIndex++)
{
btTransform childTrans;
childTrans.setIdentity();
visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type;
char relativeFileName[1024];
char pathPrefix[1024];
@@ -4106,41 +4158,23 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures);
}
if (vertices.size() && indices.size())
{
if (1)
if (visualShapeUniqueId<0)
{
int textureIndex = -1;
if (textures.size())
{
textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
}
int graphicsIndex = -1;
{
B3_PROFILE("registerGraphicsShape");
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
if (graphicsIndex >= 0)
{
int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
visualHandle->m_OpenGLGraphicsIndex = graphicsIndex;
visualHandle->m_tinyRendererVisualShapeIndex = -1;
//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
//store needed info for tinyrenderer
visualHandle->m_localInertiaFrame = localInertiaFrame;
visualHandle->m_visualShape = visualShape;
visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : "";
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
}
}
visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
}
InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
visualHandle->m_OpenGLGraphicsIndex = -1;
visualHandle->m_tinyRendererVisualShapeIndex = -1;
//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
//store needed info for tinyrenderer
visualShape.m_linkLocalFrame = childTrans;
visualHandle->m_visualShapes.push_back(visualShape);
visualHandle->m_pathPrefixes.push_back(pathPrefix[0] ? pathPrefix : "");
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
}
return hasStatus;