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

Merge pull request #1025 from olegklimov/master

Browse Source 
URDF loader improvement 2
This commit is contained in:
erwincoumans
2017-03-21 17:12:22 -07:00
committed by GitHub
parent 2b698688f4 80a0b51a1d
commit a30b887595
62 changed files with 398 additions and 331 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds)
double PhysicsDirect::getTimeOut() const
{
return m_data->m_timeOutInSeconds;
}
}

25
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1499,7 +1499,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
m_data->m_sdfRecentLoadedBodies.clear();
BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter
BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
bool useFixedBase = false;
MyMJCFLogger2 logger;
@@ -4612,20 +4612,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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,
int success = 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_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
hasStatus = true;
break;
if (success) {
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_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
hasStatus = true;
break;
}
case CMD_UPDATE_VISUAL_SHAPE:
{

163
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -170,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
m_data->m_hasLightSpecularCoeff = true;
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
void convertURDFToVisualShape(const UrdfShape* 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;
@@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
visualShapeOut.m_dimensions[1] = 0;
visualShapeOut.m_dimensions[2] = 0;
visualShapeOut.m_meshAssetFileName[0] = 0;
if (visual->m_geometry.m_hasLocalMaterial) {
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
}
GLInstanceGraphicsShape* glmesh = 0;
@@ -186,25 +192,63 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
switch (visual->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
case URDF_GEOM_CAPSULE:
{
btVector3 p1 = visual->m_geometry.m_capsuleFrom;
btVector3 p2 = visual->m_geometry.m_capsuleTo;
btTransform tr;
tr.setIdentity();
btScalar rad, len;
if (visual->m_geometry.m_hasFromTo) {
btVector3 v = p2 - p1;
btVector3 center = (p2 + p1) * 0.5;
btVector3 up_vector(0,0,1);
btVector3 dir = v.normalized();
btVector3 axis = dir.cross(up_vector);
if (axis.fuzzyZero())
{
axis = btVector3(0,0,1);
}
else
{
axis.normalize();
}
btQuaternion q(axis, -acos(dir.dot(up_vector)));
btTransform capsule_orient(q, center);
tr = visual->m_linkLocalFrame * capsule_orient;
len = v.length();
rad = visual->m_geometry.m_capsuleRadius;
} else {
tr = visual->m_linkLocalFrame;
len = visual->m_geometry.m_capsuleHalfHeight;
rad = visual->m_geometry.m_capsuleRadius;
}
visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1];
visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2];
visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0];
visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1];
visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2];
visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3];
visualShapeOut.m_dimensions[0] = len;
visualShapeOut.m_dimensions[1] = rad;
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++)
{
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
btScalar cylLength = visual->m_geometry.m_cylinderLength;
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
vertices.push_back(vert);
vert[2] = -cylLength / 2.;
vert[2] = -len / 2.;
vertices.push_back(vert);
}
if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) {
// TODO: check if tiny renderer works with that, didn't check -- Oleg
btVector3 pole1(0, 0, + len / 2. + rad);
btVector3 pole2(0, 0, - len / 2. - rad);
vertices.push_back(pole1);
vertices.push_back(pole2);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
@@ -228,7 +272,7 @@ 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;
@@ -244,14 +288,6 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
switch (visual->m_geometry.m_meshFileType)
{
case UrdfGeometry::FILE_OBJ:
@@ -467,17 +503,30 @@ 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 bodyUniqueId)
void TinyRendererVisualShapeConverter::convertVisualShapes(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model,
class btCollisionObject* colObj, int bodyUniqueId)
{
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
if (linkPtr)
{
const btArray<UrdfVisual>* shapeArray;
bool useVisual;
int cnt = 0;
if (linkPtr->m_visualArray.size() > 0)
{
useVisual = true;
cnt = linkPtr->m_visualArray.size();
}
else
{
// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
useVisual = false;
cnt = linkPtr->m_collisionArray.size();
}
const UrdfLink* link = *linkPtr;
for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
for (int v1=0; v1<cnt; v1++)
{
btAlignedObjectArray<MyTexture2> textures;
btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -485,20 +534,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
btTransform startTrans; startTrans.setIdentity();
//int graphicsIndex = -1;
const UrdfVisual& vis = link->m_visualArray[v1];
btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
UrdfMaterial *const * matPtr = model.m_materials[matName];
float rgbaColor[4] = {1,1,1,1};
if (matPtr)
const UrdfShape* vis;
if (useVisual) {
vis = &linkPtr->m_visualArray[v1];
} else {
vis = &linkPtr->m_collisionArray[v1];
}
btTransform childTrans = vis->m_linkLocalFrame;
float rgbaColor[4] = {1,1,1,1};
if (model && useVisual)
{
UrdfMaterial *const mat = *matPtr;
for (int i=0;i<4;i++)
rgbaColor[i] = mat->m_rgbaColor[i];
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
UrdfMaterial*const* matPtr = model->m_materials[matName];
if (matPtr)
{
for (int i=0; i<4; i++)
{
rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
}
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
}
}
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
@@ -513,19 +570,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
b3VisualShapeData visualShape;
visualShape.m_objectUniqueId = bodyUniqueId;
visualShape.m_linkIndex = linkIndex;
visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0];
visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1];
visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2];
visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0];
visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1];
visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2];
visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3];
visualShape.m_rgbaColor[0] = rgbaColor[0];
visualShape.m_rgbaColor[1] = rgbaColor[1];
visualShape.m_rgbaColor[2] = rgbaColor[2];
visualShape.m_rgbaColor[3] = rgbaColor[3];
visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0];
visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1];
visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2];
visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0];
visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1];
visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2];
visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3];
visualShape.m_rgbaColor[0] = rgbaColor[0];
visualShape.m_rgbaColor[1] = rgbaColor[1];
visualShape.m_rgbaColor[2] = rgbaColor[2];
visualShape.m_rgbaColor[3] = rgbaColor[3];
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
m_data->m_visualShapes.push_back(visualShape);
if (vertices.size() && indices.size())

2
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex);
virtual int getNumVisualShapes(int bodyUniqueId);