AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1078 from olegklimov/master

Browse Source 
Random improvements 2
This commit is contained in:
erwincoumans 2017-05-06 16:46:59 +00:00 committed by GitHub
commit c4844c650f
5 changed files with 85 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -195,6 +195,14 @@ struct BulletMJCFImporterInternalData
m_pathPrefix[0] = 0;
}
~BulletMJCFImporterInternalData()
{
for (int i=0;i<m_models.size();i++)
{
delete m_models[i];
}
}
std::string sourceFileLocation(TiXmlElement* e)
{
#if 0
@ -630,11 +638,7 @@ struct BulletMJCFImporterInternalData
bool handledGeomType = false;
UrdfGeometry geom;
const char* gType = link_xml->Attribute("type");
const char* sz = link_xml->Attribute("size");
const char* posS = link_xml->Attribute("pos");
int conDim = defaults.m_defaultConDim;
const char* conDimS = link_xml->Attribute("condim");
@ -656,7 +660,7 @@ struct BulletMJCFImporterInternalData
btArray<float> frictions;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, sz, strArray);
urdfStringSplit(pieces, frictionS, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
@ -707,6 +711,7 @@ struct BulletMJCFImporterInternalData
geom.m_localMaterial.m_name = rgba;
}
const char* posS = link_xml->Attribute("pos");
if (posS)
{
btVector3 pos(0,0,0);
@ -716,18 +721,32 @@ struct BulletMJCFImporterInternalData
linkLocalFrame.setOrigin(pos);
}
}
const char* ornS = link_xml->Attribute("quat");
if (ornS)
{
std::string ornStr = ornS;
btQuaternion orn(0,0,0,1);
btVector4 o4;
if (parseVector4(o4,ornStr))
if (parseVector4(o4, ornS))
{
orn.setValue(o4[1],o4[2],o4[3],o4[0]);
linkLocalFrame.setRotation(orn);
}
}
const char* axis_and_angle = link_xml->Attribute("axisangle");
if (axis_and_angle)
{
btQuaternion orn(0,0,0,1);
btVector4 o4;
if (parseVector4(o4, axis_and_angle))
{
orn.setRotation(btVector3(o4[0],o4[1],o4[2]), o4[3]);
linkLocalFrame.setRotation(orn);
}
}
const char* gType = link_xml->Attribute("type");
if (gType)
{
std::string geomType = gType;
@ -792,8 +811,8 @@ struct BulletMJCFImporterInternalData
}
}
geom.m_capsuleRadius = 0;
geom.m_capsuleHeight = 0.f;
geom.m_capsuleRadius = 2.00f; // 2 to make it visible if something is wrong
geom.m_capsuleHeight = 2.00f;
if (sizes.size()>0)
{
@ -877,6 +896,7 @@ struct BulletMJCFImporterInternalData
col.m_geometry = geom;
col.m_linkLocalFrame = linkLocalFrame;
col.m_sourceFileLocation = sourceFileLocation(link_xml);
linkPtr->m_collisionArray.push_back(col);
} else
@ -950,22 +970,6 @@ struct BulletMJCFImporterInternalData
col->m_geometry.m_boxSize[2];
break;
}
case URDF_GEOM_CYLINDER:
{
double r = col->m_geometry.m_capsuleRadius;
btScalar h(0);
//and one cylinder of 'height'
if (col->m_geometry.m_hasFromTo)
{
h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length();
} else
{
h = col->m_geometry.m_capsuleHeight;
}
totalVolume += SIMD_PI*r*r*h;
break;
}
case URDF_GEOM_MESH:
{
//todo (based on mesh bounding box?)
@ -976,11 +980,15 @@ struct BulletMJCFImporterInternalData
//todo
break;
}
case URDF_GEOM_CYLINDER:
case URDF_GEOM_CAPSULE:
{
//one sphere
double r = col->m_geometry.m_capsuleRadius;
totalVolume += 4./3.*SIMD_PI*r*r*r;
if (col->m_geometry.m_type==URDF_GEOM_CAPSULE)
{
totalVolume += 4./3.*SIMD_PI*r*r*r;
}
btScalar h(0);
if (col->m_geometry.m_hasFromTo)
{
@ -1017,9 +1025,9 @@ struct BulletMJCFImporterInternalData
UrdfModel* modelPtr = m_models[modelIndex];
int orgChildLinkIndex = modelPtr->m_links.size();
UrdfLink* linkPtr = new UrdfLink();
char uniqueLinkName[1024];
sprintf(uniqueLinkName,"link%d",orgChildLinkIndex );
linkPtr->m_name = uniqueLinkName;
char linkn[1024];
sprintf(linkn, "link%d_%d", modelIndex, orgChildLinkIndex);
linkPtr->m_name = linkn;
if (namePtr)
{
linkPtr->m_name = namePtr;
@ -1205,6 +1213,7 @@ struct BulletMJCFImporterInternalData
}
linkPtr->m_linkTransformInWorld = linkTransform;
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
{
//linkPtr->m_linkTransformInWorld.setIdentity();

3
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -285,6 +285,9 @@ void ConvertURDF2BulletInternal(
if (!(flags & CUF_USE_URDF_INERTIA))
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
btAssert(localInertiaDiagonal[0] < 1e10);
btAssert(localInertiaDiagonal[1] < 1e10);
btAssert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);

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

@ -13,48 +13,10 @@ m_activeSdfModel(-1)
UrdfParser::~UrdfParser()
{
cleanModel(&m_urdf2Model);
for (int i=0;i<m_tmpModels.size();i++)
{
cleanModel(m_tmpModels[i]);
for (int i=0;i<m_tmpModels.size();i++)
{
delete m_tmpModels[i];
}
m_sdfModels.clear();
m_tmpModels.clear();
}
void UrdfParser::cleanModel(UrdfModel* model)
{
for (int i=0;i<model->m_materials.size();i++)
{
UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
if (matPtr)
{
UrdfMaterial* mat = *matPtr;
delete mat;
}
}
for (int i=0;i<model->m_links.size();i++)
{
UrdfLink** linkPtr = model->m_links.getAtIndex(i);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
delete link;
}
}
for (int i=0;i<model->m_joints.size();i++)
{
UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
delete joint;
}
}
}
}
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
@ -1471,6 +1433,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
delete material;
logger->reportWarning("Duplicate material");
} else
{
@ -1494,6 +1457,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
delete link;
return false;
} else
{
@ -1542,6 +1506,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
delete joint;
return false;
}
else
@ -1552,6 +1517,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
else
{
logger->reportError("joint xml is not initialized correctly");
delete joint;
return false;
}
}
@ -1666,7 +1632,8 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
logger->reportWarning("Duplicate material");
delete material;
} else
{
localModel->m_materials.insert(material->m_name.c_str(),material);
@ -1689,6 +1656,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
delete link;
return false;
} else
{
@ -1737,6 +1705,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
delete joint;
return false;
}
else
@ -1747,6 +1716,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
else
{
logger->reportError("joint xml is not initialized correctly");
delete joint;
return false;
}
}

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

@ -195,7 +195,37 @@ struct UrdfModel
{
m_rootTransformInWorld.setIdentity();
}
~UrdfModel()
{
for (int i = 0; i < m_materials.size(); i++)
{
UrdfMaterial** ptr = m_materials.getAtIndex(i);
if (ptr)
{
UrdfMaterial* t = *ptr;
delete t;
}
}
for (int i = 0; i < m_links.size(); i++)
{
UrdfLink** ptr = m_links.getAtIndex(i);
if (ptr)
{
UrdfLink* t = *ptr;
delete t;
}
}
for (int i = 0; i < m_joints.size(); i++)
{
UrdfJoint** ptr = m_joints.getAtIndex(i);
if (ptr)
{
UrdfJoint* t = *ptr;
delete t;
}
}
}
};
class UrdfParser
@ -210,7 +240,6 @@ protected:
int m_activeSdfModel;
void cleanModel(UrdfModel* model);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);

2
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@ -526,7 +526,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
}
else
{
// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
// We have to see something, take collision shape. Useful for MuJoCo xml, where there are no explicit visual shapes.
useVisual = false;
cnt = linkPtr->m_collisionArray.size();
}