From f86ce4d44b9a8dd01e906699d450b7b129464932 Mon Sep 17 00:00:00 2001 From: Logan Su Date: Mon, 29 Aug 2016 15:09:18 -0700 Subject: [PATCH] Add flags to load inertia values from URDF. --- .../ImportSDFDemo/ImportSDFSetup.cpp | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 26 ++++++++++++++----- .../Importers/ImportURDFDemo/URDF2Bullet.h | 15 +++++++---- .../PhysicsServerCommandProcessor.cpp | 2 +- 4 files changed, 31 insertions(+), 14 deletions(-) diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index f55cc9758..6e8437950 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -232,7 +232,7 @@ void ImportSDFSetup::initPhysics() MyMultiBodyCreator creation(m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true); + ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF); mb = creation.getBulletMultiBody(); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8503dd41d..c3aaee49b 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -1,4 +1,3 @@ -#include "URDFImporterInterface.h" #include #include "LinearMath/btTransform.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" @@ -7,6 +6,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "URDF2Bullet.h" #include "URDFImporterInterface.h" #include "MultiBodyCreationInterface.h" #include @@ -143,7 +143,12 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat } -void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) +void ConvertURDF2BulletInternal( + const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, + URDF2BulletCachedData& cache, int urdfLinkIndex, + const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, + bool createMultiBody, const char* pathPrefix, + int flags = 0) { //b3Printf("start converting/extracting data from URDF interface\n"); @@ -200,7 +205,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); - if (useSDF) + if (flags & CUF_USE_SDF) { parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; } @@ -231,7 +236,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat */ if (mass) { - compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + if (!(flags & CUF_USE_URDF_INERTIA)) + { + compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); //temporary inertia scaling until we load inertia from URDF @@ -436,18 +444,22 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); + ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); } } -void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) +void ConvertURDF2Bullet( + const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, + const btTransform& rootTransformInWorldSpace, + btMultiBodyDynamicsWorld* world1, + bool createMultiBody, const char* pathPrefix, int flags) { URDF2BulletCachedData cache; InitURDF2BulletCache(u2b,cache); int urdfLinkIndex = u2b.getRootLinkIndex(); - ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); if (world1 && cache.m_bulletMultiBody) { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 148535812..dcf05c6a0 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -14,14 +14,19 @@ class MultiBodyCreationInterface; +enum ConvertURDFFlags { + CUF_USE_SDF = 1, + // Use inertia values in URDF instead of recomputing them from collision shape. + CUF_USE_URDF_INERTIA = 2 +}; -void ConvertURDF2Bullet(const URDFImporterInterface& u2b, - MultiBodyCreationInterface& creationCallback, - const btTransform& rootTransformInWorldSpace, +void ConvertURDF2Bullet(const URDFImporterInterface& u2b, + MultiBodyCreationInterface& creationCallback, + const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world, - bool createMultiBody, + bool createMultiBody, const char* pathPrefix, - bool useSDF = false); + int flags = 0); #endif //_URDF2BULLET_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3c30f30df..84d4554ca 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -776,7 +776,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe MyMultiBodyCreator creation(m_data->m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF);