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

also generate TinyRendererVisualShapeConverter for programmatically generated collision shapes

Browse Source 
use similar random colors for TinyRenderer (if rgba colors are not specified)
This commit is contained in:
Erwin Coumans
2017-06-29 22:06:27 -07:00
parent bf800e30d5
commit dcaaed9238
3 changed files with 85 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -156,7 +156,8 @@ struct UrdfLink
UrdfLink() UrdfLink()
:m_parentLink(0), :m_parentLink(0),
m_parentJoint(0) m_parentJoint(0),
m_linkIndex(-2)
{ {
} }

79
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -132,6 +132,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
struct InteralCollisionShapeData struct InteralCollisionShapeData
{ {
btCollisionShape* m_collisionShape; btCollisionShape* m_collisionShape;
b3AlignedObjectArray<UrdfCollision> m_urdfCollisionObjects;
void clear() void clear()
{ {
m_collisionShape=0; m_collisionShape=0;
@@ -1739,21 +1740,28 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
return -1; return -1;
} }
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const 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...
#if 0
if (m_data->m_customVisualShapesConverter) UrdfModel model;// = m_data->m_urdfParser.getModel();
UrdfLink link;
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
if (colShapeUniqueId>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
{ {
const UrdfModel& model = m_data->m_urdfParser.getModel(); for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (linkPtr)
{ {
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
} }
} }
} }
#endif //UrdfVisual vis;
//link.m_visualArray.push_back(vis);
//UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
m_data->m_visualConverter.convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, &link, &model, colObj, bodyUniqueId);
} }
virtual void setBodyUniqueId(int bodyId) virtual void setBodyUniqueId(int bodyId)
{ {
@@ -3588,6 +3596,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
btCollisionShape* shape = 0; btCollisionShape* shape = 0;
b3AlignedObjectArray<UrdfCollision> urdfCollisionObjects;
btCompoundShape* compound = 0; btCompoundShape* compound = 0;
@@ -3597,6 +3606,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
for (int i=0;i<clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes;i++) for (int i=0;i<clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes;i++)
{ {
UrdfCollision urdfColObj;
btTransform childTransform; btTransform childTransform;
childTransform.setIdentity(); childTransform.setIdentity();
if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_hasChildTransform) if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_hasChildTransform)
@@ -3616,6 +3627,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
urdfColObj.m_linkLocalFrame = childTransform;
urdfColObj.m_sourceFileLocation = "memory";
urdfColObj.m_name = "memory";
urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN;
switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type) switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type)
{ {
case GEOM_SPHERE: case GEOM_SPHERE:
@@ -3626,19 +3642,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
compound->addChildShape(childTransform,shape); compound->addChildShape(childTransform,shape);
} }
urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
urdfColObj.m_geometry.m_sphereRadius = radius;
break; break;
} }
case GEOM_BOX: case GEOM_BOX:
{ {
//double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; //double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createBoxShape(btVector3( btVector3 halfExtents(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0], clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1], clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2])); clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
shape = worldImporter->createBoxShape(halfExtents);
if (compound) if (compound)
{ {
compound->addChildShape(childTransform,shape); compound->addChildShape(childTransform,shape);
} }
urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
break; break;
} }
case GEOM_CAPSULE: case GEOM_CAPSULE:
@@ -3649,6 +3670,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
compound->addChildShape(childTransform,shape); compound->addChildShape(childTransform,shape);
} }
urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight;
break; break;
} }
case GEOM_CYLINDER: case GEOM_CYLINDER:
@@ -3659,6 +3684,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
compound->addChildShape(childTransform,shape); compound->addChildShape(childTransform,shape);
} }
urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight;
break; break;
} }
case GEOM_PLANE: case GEOM_PLANE:
@@ -3672,6 +3701,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
compound->addChildShape(childTransform,shape); compound->addChildShape(childTransform,shape);
} }
urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
urdfColObj.m_geometry.m_planeNormal.setValue(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]);
break; break;
} }
case GEOM_MESH: case GEOM_MESH:
@@ -3685,7 +3720,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
const std::string& urdf_path=""; const std::string& urdf_path="";
std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName; std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName;
urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
urdfColObj.m_geometry.m_meshFileName = fileName;
urdfColObj.m_geometry.m_meshScale = meshScale;
char relativeFileName[1024]; char relativeFileName[1024];
char pathPrefix[1024]; char pathPrefix[1024];
pathPrefix[0] = 0; pathPrefix[0] = 0;
@@ -3702,6 +3740,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
if (foundFile) if (foundFile)
{ {
urdfColObj.m_geometry.m_meshFileType = out_type;
if (out_type==UrdfGeometry::FILE_OBJ) if (out_type==UrdfGeometry::FILE_OBJ)
{ {
//create a convex hull for each shape, and store it in a btCompoundShape //create a convex hull for each shape, and store it in a btCompoundShape
@@ -3718,10 +3758,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
btAlignedObjectArray<btVector3> convertedVerts; btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices); convertedVerts.reserve(glmesh->m_numvertices);
btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]);
for (int i=0; i<glmesh->m_numvertices; i++) for (int i=0; i<glmesh->m_numvertices; i++)
{ {
convertedVerts.push_back(btVector3( convertedVerts.push_back(btVector3(
@@ -3809,7 +3846,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
} }
} }
if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN)
{
urdfCollisionObjects.push_back(urdfColObj);
}
} }
#if 0 #if 0
shape = worldImporter->createCylinderShapeX(radius,height); shape = worldImporter->createCylinderShapeX(radius,height);
shape = worldImporter->createCylinderShapeY(radius,height); shape = worldImporter->createCylinderShapeY(radius,height);
@@ -3829,6 +3872,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle();
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid);
handle->m_collisionShape = shape; handle->m_collisionShape = shape;
for (int i=0;i<urdfCollisionObjects.size();i++)
{
handle->m_urdfCollisionObjects.push_back(urdfCollisionObjects[i]);
}
serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid; serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid;
m_data->m_worldImporters.push_back(worldImporter); m_data->m_worldImporters.push_back(worldImporter);
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;

21
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -506,6 +506,15 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
} }
static btVector4 sColors[4] =
{
btVector4(60./256.,186./256.,84./256.,1),
btVector4(244./256.,194./256.,13./256.,1),
btVector4(219./256.,50./256.,54./256.,1),
btVector4(72./256.,133./256.,237./256.,1),
//btVector4(1,1,0,1),
};
void TinyRendererVisualShapeConverter::convertVisualShapes( void TinyRendererVisualShapeConverter::convertVisualShapes(
@@ -546,7 +555,17 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
} }
btTransform childTrans = vis->m_linkLocalFrame; btTransform childTrans = vis->m_linkLocalFrame;
float rgbaColor[4] = {1,1,1,1};
int colorIndex = colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0;
btVector4 color;
color = sColors[colorIndex];
float rgbaColor[4] = {color[0],color[1],color[2],color[3]};
if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE)
{
color.setValue(1,1,1,1);
}
if (model && useVisual) if (model && useVisual)
{ {
btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());