From 84b20bda80e8fa0898f6f1372f3902fe8afa98a6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 3 May 2018 14:24:16 -0700 Subject: [PATCH] don't use the URDF inertia element, unless flag CUF_USE_URDF_INERTIA is set, not for the diagonal and also not for the inertial frame shift. --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 39 +++++++++++++++++++ .../ImportURDFDemo/BulletUrdfImporter.h | 2 + .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 4 +- .../ImportURDFDemo/URDFImporterInterface.h | 8 +++- .../PhysicsServerCommandProcessor.cpp | 4 +- 5 files changed, 51 insertions(+), 6 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 3f6043d06..20b1e4370 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -300,6 +300,45 @@ std::string BulletURDFImporter::getJointName(int linkIndex) const return ""; } +void BulletURDFImporter::getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const +{ + if (flags & CUF_USE_URDF_INERTIA) + { + getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame); + } + else + { + //the link->m_inertia is NOT necessarily aligned with the inertial frame + //so an additional transform might need to be computed + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex); + + btAssert(linkPtr); + if (linkPtr) + { + UrdfLink* link = *linkPtr; + btScalar linkMass; + if (link->m_parentJoint == 0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) + { + linkMass = 0.f; + } + else + { + linkMass = link->m_inertia.m_mass; + } + mass = linkMass; + localInertiaDiagonal.setValue(0,0,0); + inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin()); + inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()); + } + else + { + mass = 1.f; + localInertiaDiagonal.setValue(1, 1, 1); + inertialFrame.setIdentity(); + } + } + +} void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 7de13f35d..e058c56ea 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -59,6 +59,8 @@ public: virtual std::string getJointName(int linkIndex) const; virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; + virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const; virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 1db703604..8bf13d865 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -215,7 +215,7 @@ void ConvertURDF2BulletInternal( //b3Printf("urdf parent index = %d\n",urdfParentIndex); //b3Printf("mb parent index = %d\n",mbParentIndex); parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); - u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame); + u2b.getMassAndInertia2(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame, flags); } @@ -223,7 +223,7 @@ void ConvertURDF2BulletInternal( btTransform localInertialFrame; localInertialFrame.setIdentity(); btVector3 localInertiaDiagonal(0,0,0); - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); + u2b.getMassAndInertia2(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame, flags); diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 581c55fc8..703303dfe 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -48,8 +48,12 @@ public: virtual std::string getJointName(int linkIndex) const = 0; //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. - virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0; - + virtual void getMassAndInertia (int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0; + virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const + { + getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame); + } + ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const =0; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 018463307..6418648e6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2602,7 +2602,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, bodyHandle->m_bodyName = u2b.getBodyName(); btVector3 localInertiaDiagonal(0,0,0); int urdfLinkIndex = u2b.getRootLinkIndex(); - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); + u2b.getMassAndInertia2(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame,flags); } @@ -2664,7 +2664,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, btScalar mass; btVector3 localInertiaDiagonal(0,0,0); btTransform localInertialFrame; - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); + u2b.getMassAndInertia2(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame, flags); bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());