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

URDF loader: fix MuJoCo xml load, also closes #993

Browse Source
This commit is contained in:
Oleg Klimov
2017-03-17 02:09:55 +03:00
parent 86bec45247
commit e8da7bb6f8
13 changed files with 259 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
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;
@@ -211,6 +211,38 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
convexColShape = cylZShape;
break;
}
case URDF_GEOM_CAPSULE:
{
btVector3 p1 = visual->m_geometry.m_capsuleFrom;
btVector3 p2 = visual->m_geometry.m_capsuleTo;
btVector3 v = p2 - p1;
btVector3 center = (p2 + p1) * 0.5;
btScalar rad = visual->m_geometry.m_capsuleRadius;
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);
btTransform tr = visual->m_linkLocalFrame * capsule_orient;
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] = v.length();
visualShapeOut.m_dimensions[1] = rad;
break;
}
case URDF_GEOM_BOX:
{
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0];
@@ -228,7 +260,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 +276,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 +491,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 +522,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 +558,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())