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

MJCF: fix B3_PI, fix colors, fix capsule without 'fromto'

Browse Source
This commit is contained in:
Oleg Klimov
2017-03-19 00:19:04 +03:00
parent 4526b0a94a
commit 41df15a464
4 changed files with 75 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -155,6 +155,9 @@ struct BulletMJCFImporterInternalData
// joint defaults
std::string m_defaultJointLimited;
// geom defaults
std::string m_defaultGeomRgba;
//those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
@@ -270,6 +273,11 @@ struct BulletMJCFImporterInternalData
{
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
}
const char* rgba = child_xml->Attribute("rgba");
if (rgba)
{
m_defaultGeomRgba = rgba;
}
}
}
handled=true;
@@ -404,8 +412,8 @@ struct BulletMJCFImporterInternalData
if (sizes.size()==2)
{
// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
range[0] = sizes[0] * M_PI / 180;
range[1] = sizes[1] * M_PI / 180;
range[0] = sizes[0] * B3_PI / 180;
range[1] = sizes[1] * B3_PI / 180;
} else
{
logger->reportWarning("Expected range[2] in joint with limits");
@@ -526,10 +534,23 @@ struct BulletMJCFImporterInternalData
// const char* rgba = link_xml->Attribute("rgba");
const char* gType = link_xml->Attribute("type");
const char* sz = link_xml->Attribute("size");
const char* posS = link_xml->Attribute("pos");
std::string rgba = m_defaultGeomRgba;
if (const char* rgbattr = link_xml->Attribute("rgba"))
{
rgba = rgbattr;
}
if (!rgba.empty())
{
// "0 0.7 0.7 1"
parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
geom.m_hasLocalMaterial = true;
geom.m_localMaterial.m_name = rgba;
}
if (posS)
{
btVector3 pos(0,0,0);

18
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -575,7 +575,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
visual.m_geometry.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
@@ -597,7 +597,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
matPtr->m_rgbaColor = rgba;
visual.m_materialName = matPtr->m_name;
visual.m_hasLocalMaterial = true;
visual.m_geometry.m_hasLocalMaterial = true;
}
}
else
@@ -616,11 +616,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
visual.m_geometry.m_hasLocalMaterial = true;
}
}
}
@@ -1408,12 +1408,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
vis.m_geometry.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");
@@ -1603,12 +1603,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
vis.m_geometry.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");

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

@@ -20,7 +20,11 @@ struct UrdfMaterial
{
std::string m_name;
std::string m_textureFilename;
btVector4 m_rgbaColor;
btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
UrdfMaterial():
m_rgbaColor(0.8, 0.8, 0.8, 1)
{
}
};
struct UrdfInertia
@@ -79,6 +83,9 @@ struct UrdfGeometry
int m_meshFileType;
std::string m_meshFileName;
btVector3 m_meshScale;
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;
};
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
@@ -96,8 +103,6 @@ struct UrdfShape
struct UrdfVisual: UrdfShape
{
std::string m_materialName;
bool m_hasLocalMaterial;
UrdfMaterial m_localMaterial;
};
struct UrdfCollision: UrdfShape

22
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
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;
@@ -215,9 +221,12 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
{
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;
btScalar rad = visual->m_geometry.m_capsuleRadius;
btVector3 up_vector(0,0,1);
btVector3 dir = v.normalized();
btVector3 axis = dir.cross(up_vector);
@@ -231,7 +240,14 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
}
btQuaternion q(axis, -acos(dir.dot(up_vector)));
btTransform capsule_orient(q, center);
btTransform tr = visual->m_linkLocalFrame * capsule_orient;
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];
@@ -239,7 +255,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
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[0] = len;
visualShapeOut.m_dimensions[1] = rad;
break;
}