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

Merge pull request #763 from ludi1001/enable-load-inertia-from-urdf

Browse Source 
Add flags to load inertia values from URDF.
This commit is contained in:
erwincoumans
2016-08-30 08:59:29 -07:00
committed by GitHub
parent fb57689fad f86ce4d44b
commit 4ebd6b144b
4 changed files with 31 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -232,7 +232,7 @@ void ImportSDFSetup::initPhysics()
MyMultiBodyCreator creation(m_guiHelper); MyMultiBodyCreator creation(m_guiHelper);
u2b.getRootTransformInWorld(rootTrans); 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(); mb = creation.getBulletMultiBody();

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

@@ -1,4 +1,3 @@
#include "URDFImporterInterface.h"
#include <stdio.h> #include <stdio.h>
#include "LinearMath/btTransform.h" #include "LinearMath/btTransform.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@@ -7,6 +6,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "URDF2Bullet.h"
#include "URDFImporterInterface.h" #include "URDFImporterInterface.h"
#include "MultiBodyCreationInterface.h" #include "MultiBodyCreationInterface.h"
#include <string> #include <string>
@@ -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"); //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); bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
if (useSDF) if (flags & CUF_USE_SDF)
{ {
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
} }
@@ -231,7 +236,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
*/ */
if (mass) if (mass)
{ {
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); if (!(flags & CUF_USE_URDF_INERTIA))
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
}
URDFLinkContactInfo contactInfo; URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
//temporary inertia scaling until we load inertia from URDF //temporary inertia scaling until we load inertia from URDF
@@ -436,18 +444,22 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{ {
int urdfChildLinkIndex = urdfChildIndices[i]; 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; URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache); InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex(); 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) if (world1 && cache.m_bulletMultiBody)
{ {

15
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -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, void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback, MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace, const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world, btMultiBodyDynamicsWorld* world,
bool createMultiBody, bool createMultiBody,
const char* pathPrefix, const char* pathPrefix,
bool useSDF = false); int flags = 0);
#endif //_URDF2BULLET_H #endif //_URDF2BULLET_H

2
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -776,7 +776,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
MyMultiBodyCreator creation(m_data->m_guiHelper); MyMultiBodyCreator creation(m_data->m_guiHelper);
u2b.getRootTransformInWorld(rootTrans); 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);