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

MJCF: support for "axisangle", small code improvements

Browse Source
This commit is contained in:
Oleg Klimov
2017-04-06 04:25:56 +03:00
parent 49eda303bc
commit 1524004aa8
3 changed files with 30 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -630,11 +630,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");
@@ -707,6 +703,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 +713,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 +803,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 +888,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 +962,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 +972,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)
{
@@ -1205,6 +1205,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);
assert(localInertiaDiagonal[0] < 1e10);
assert(localInertiaDiagonal[1] < 1e10);
assert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);

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();
}