AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-08 08:30:16 +00:00
Code Issues Releases Wiki Activity

Merge pull request #783 from bingjeff/urdf_loader_changes

Browse Source 
[URDF] Edit loader to handle full inertia tensor.
This commit is contained in:
erwincoumans 2016-09-20 04:39:38 -07:00 committed by GitHub
commit 8d4c99801e
1 changed files with 52 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -295,19 +295,59 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
if (linkPtr) if (linkPtr)
{ {
UrdfLink* link = *linkPtr; UrdfLink* link = *linkPtr;
btMatrix3x3 linkInertiaBasis;
btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{ {
mass = 0.f; linkMass = 0.f;
localInertiaDiagonal.setValue(0,0,0); principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
} }
else else
{ {
mass = link->m_inertia.m_mass; linkMass = link->m_inertia.m_mass;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy, if (link->m_inertia.m_ixy == 0.0 &&
link->m_inertia.m_izz); link->m_inertia.m_ixz == 0.0 &&
link->m_inertia.m_iyz == 0.0)
{
principalInertiaX = link->m_inertia.m_ixx;
principalInertiaY = link->m_inertia.m_iyy;
principalInertiaZ = link->m_inertia.m_izz;
linkInertiaBasis.setIdentity();
} }
inertialFrame = link->m_inertia.m_linkLocalFrame; else
{
principalInertiaX = link->m_inertia.m_ixx;
btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
btScalar threshold = 1.0e-6;
int numIterations = 30;
inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
principalInertiaX = inertiaTensor[0][0];
principalInertiaY = inertiaTensor[1][1];
principalInertiaZ = inertiaTensor[2][2];
}
}
mass = linkMass;
if (principalInertiaX < 0 ||
principalInertiaX > (principalInertiaY + principalInertiaZ) ||
principalInertiaY < 0 ||
principalInertiaY > (principalInertiaX + principalInertiaZ) ||
principalInertiaZ < 0 ||
principalInertiaZ > (principalInertiaX + principalInertiaY))
{
b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
inertialFrame.setBasis(linkInertiaBasis * link->m_inertia.m_linkLocalFrame.getBasis());
} }
else else
{ {