mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
more URDF2Bullet refactor to make URDF import a little bit more re-usable
This commit is contained in:
Erwin Coumans
parent
3d19aec7fa
commit
20a270bc94
@ -2,6 +2,7 @@
|
||||
#include "ImportURDFSetup.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
#include "../ImportObjDemo/LoadMeshFromObj.h"
|
||||
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
|
||||
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
@ -33,11 +34,13 @@ class MyURDF2Bullet : public URDF2Bullet
|
||||
my_shared_ptr<ModelInterface> m_robot;
|
||||
std::vector<my_shared_ptr<Link> > m_links;
|
||||
GraphicsPhysicsBridge& m_gfxBridge;
|
||||
mutable btMultiBody* m_bulletMultiBody;
|
||||
|
||||
public:
|
||||
MyURDF2Bullet(my_shared_ptr<ModelInterface> robot,GraphicsPhysicsBridge& gfxBridge)
|
||||
:m_robot(robot),
|
||||
m_gfxBridge(gfxBridge)
|
||||
m_gfxBridge(gfxBridge),
|
||||
m_bulletMultiBody(0)
|
||||
{
|
||||
m_robot->getLinks(m_links);
|
||||
|
||||
@ -53,7 +56,7 @@ public:
|
||||
if (m_links.size())
|
||||
{
|
||||
int rootLinkIndex = m_robot->getRoot()->m_link_index;
|
||||
btAssert(m_links[0]->m_link_index == rootLinkIndex);
|
||||
// btAssert(m_links[0]->m_link_index == rootLinkIndex);
|
||||
return rootLinkIndex;
|
||||
}
|
||||
return -1;
|
||||
@ -92,7 +95,7 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
virtual bool getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
|
||||
{
|
||||
jointLowerLimit = 0.f;
|
||||
jointUpperLimit = 0.f;
|
||||
@ -146,7 +149,7 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
|
||||
{
|
||||
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
@ -175,7 +178,7 @@ public:
|
||||
|
||||
}
|
||||
|
||||
virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
|
||||
{
|
||||
|
||||
btCompoundShape* compoundShape = new btCompoundShape();
|
||||
@ -201,6 +204,34 @@ public:
|
||||
return compoundShape;
|
||||
}
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const
|
||||
{
|
||||
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const
|
||||
{
|
||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
||||
rbci.m_startWorldTransform = initialWorldTrans;
|
||||
btRigidBody* body = new btRigidBody(rbci);
|
||||
return body;
|
||||
}
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) const
|
||||
{
|
||||
btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
|
||||
return mbCol;
|
||||
}
|
||||
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) const
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const
|
||||
{
|
||||
|
||||
@ -212,25 +243,50 @@ public:
|
||||
m_gfxBridge.createCollisionObjectGraphicsObject(colObj,colorRgba);
|
||||
}
|
||||
|
||||
btMultiBody* getBulletMultiBody()
|
||||
{
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
const char* fileNames[] =
|
||||
{
|
||||
"r2d2.urdf",
|
||||
};
|
||||
btAlignedObjectArray<std::string> gFileNameArray;
|
||||
|
||||
ImportUrdfSetup::ImportUrdfSetup()
|
||||
{
|
||||
static int count = 0;
|
||||
gFileNameArray.clear();
|
||||
gFileNameArray.push_back("r2d2.urdf");
|
||||
|
||||
sprintf(m_fileName,fileNames[count++]);
|
||||
int sz = sizeof(fileNames)/sizeof(char*);
|
||||
if (count>=sz)
|
||||
//load additional urdf file names from file
|
||||
|
||||
FILE* f = fopen("urdf_files.txt","r");
|
||||
if (f)
|
||||
{
|
||||
int result;
|
||||
//warning: we don't avoid string buffer overflow in this basic example in fscanf
|
||||
char fileName[1024];
|
||||
do
|
||||
{
|
||||
result = fscanf(f,"%s",fileName);
|
||||
if (result==1)
|
||||
{
|
||||
gFileNameArray.push_back(fileName);
|
||||
}
|
||||
} while (result==1);
|
||||
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
int numFileNames = gFileNameArray.size();
|
||||
|
||||
if (count>=numFileNames)
|
||||
{
|
||||
count=0;
|
||||
}
|
||||
sprintf(m_fileName,gFileNameArray[count++].c_str());
|
||||
}
|
||||
|
||||
ImportUrdfSetup::~ImportUrdfSetup()
|
||||
@ -346,7 +402,8 @@ struct URDF2BulletMappings
|
||||
enum MyFileType
|
||||
{
|
||||
FILE_STL=1,
|
||||
FILE_COLLADA=2
|
||||
FILE_COLLADA=2,
|
||||
FILE_OBJ=3,
|
||||
};
|
||||
|
||||
|
||||
@ -431,6 +488,10 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", pathPrefix, filename);
|
||||
@ -443,6 +504,12 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath,pathPrefix);
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
@ -531,6 +598,8 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -723,7 +792,10 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
FILE* f = fopen(fullPath,"rb");
|
||||
@ -735,6 +807,11 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath,pathPrefix);
|
||||
break;
|
||||
}
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
@ -821,6 +898,8 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Unsupported file type in Collision: %s\n",fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1422,7 +1501,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
|
||||
//now print the tree using the new interface
|
||||
MyURDF2Bullet u2b(robot,gfxBridge);
|
||||
u2b.printTree(0,0);
|
||||
printTree(u2b, 0,0);
|
||||
|
||||
btTransform identityTrans;
|
||||
identityTrans.setIdentity();
|
||||
@ -1434,7 +1513,8 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
|
||||
{
|
||||
URDF2BulletMappings mappings;
|
||||
URDF2BulletCachedData cache;
|
||||
|
||||
btMultiBody* mb = 0;
|
||||
|
||||
if (!useUrdfInterfaceClass)
|
||||
{
|
||||
@ -1442,25 +1522,21 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
mappings.m_totalNumJoints = numJoints;
|
||||
mappings.m_urdfLinkIndices2BulletLinkIndices.resize(numJoints+1,-2);//root and child links (=1+numJoints)
|
||||
URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
|
||||
mb = mappings.m_bulletMultiBody;
|
||||
} else
|
||||
{
|
||||
|
||||
URDF2BulletConfig config;
|
||||
config.m_createMultiBody = useFeatherstone;
|
||||
//ConvertURDF2Bullet(URDF2Bullet& u2b, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,URDF2BulletConfig& config, const char* pathPrefix)
|
||||
|
||||
|
||||
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
ConvertURDF2Bullet(u2b,identityTrans,m_dynamicsWorld,useFeatherstone,pathPrefix);
|
||||
mb = u2b.getBulletMultiBody();
|
||||
|
||||
|
||||
InitURDF2BulletCache(u2b,cache);
|
||||
ConvertURDF2Bullet(u2b,cache,rootLinkIndex,identityTrans,m_dynamicsWorld,config,pathPrefix);
|
||||
}
|
||||
if (useFeatherstone)
|
||||
{
|
||||
btMultiBody* mb = useUrdfInterfaceClass? cache.m_bulletMultiBody : mappings.m_bulletMultiBody;
|
||||
mb->setHasSelfCollision(false);
|
||||
mb->setHasSelfCollision(false);
|
||||
mb->finalizeMultiDof();
|
||||
m_dynamicsWorld->addMultiBody(mb);
|
||||
}
|
||||
@ -1482,7 +1558,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
gfxBridge.createCollisionShapeGraphicsObject(box);
|
||||
btTransform start; start.setIdentity();
|
||||
btVector3 groundOrigin(0,0,0);
|
||||
groundOrigin[upAxis]=-1.5;
|
||||
groundOrigin[upAxis]=-2;//.5;
|
||||
start.setOrigin(groundOrigin);
|
||||
btRigidBody* body = createRigidBody(0,start,box);
|
||||
//m_dynamicsWorld->removeRigidBody(body);
|
||||
|
||||
@ -30,10 +30,10 @@ static btVector3 selectColor2()
|
||||
return color;
|
||||
}
|
||||
|
||||
void URDF2Bullet::printTree(int linkIndex, int indentationLevel)
|
||||
void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
getLinkChildIndices(linkIndex,childIndices);
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
|
||||
int numChildren = childIndices.size();
|
||||
|
||||
@ -42,14 +42,68 @@ void URDF2Bullet::printTree(int linkIndex, int indentationLevel)
|
||||
for (int i=0;i<numChildren;i++)
|
||||
{
|
||||
int childLinkIndex = childIndices[i];
|
||||
std::string name = getLinkName(childLinkIndex);
|
||||
std::string name = u2b.getLinkName(childLinkIndex);
|
||||
for(int j=0;j<indentationLevel;j++) printf(" "); //indent
|
||||
printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
|
||||
// first grandchild
|
||||
printTree(childLinkIndex,indentationLevel);
|
||||
printTree(u2b,childLinkIndex,indentationLevel);
|
||||
}
|
||||
}
|
||||
|
||||
struct URDF2BulletCachedData
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
:m_totalNumJoints1(0),
|
||||
m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0)
|
||||
{
|
||||
|
||||
}
|
||||
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
||||
|
||||
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
|
||||
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
||||
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
class btMultiBody* m_bulletMultiBody;
|
||||
|
||||
//this will be initialized in the constructor
|
||||
int m_totalNumJoints1;
|
||||
int getParentUrdfIndex(int linkIndex) const
|
||||
{
|
||||
return m_urdfLinkParentIndices[linkIndex];
|
||||
}
|
||||
int getMbIndexFromUrdfIndex(int urdfIndex) const
|
||||
{
|
||||
if (urdfIndex==-2)
|
||||
return -2;
|
||||
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
|
||||
}
|
||||
|
||||
|
||||
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
|
||||
{
|
||||
return m_urdfLink2rigidBodies[urdfLinkIndex];
|
||||
}
|
||||
|
||||
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
@ -102,7 +156,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
|
||||
|
||||
}
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, const char* pathPrefix)
|
||||
void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
{
|
||||
printf("start converting/extracting data from URDF interface\n");
|
||||
|
||||
@ -156,23 +210,24 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
btScalar jointUpperLimit;
|
||||
|
||||
|
||||
bool hasParentJoint = u2b.getParent2JointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
|
||||
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
|
||||
|
||||
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
|
||||
|
||||
int graphicsIndex = u2b.convertLinkVisuals(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
|
||||
btCompoundShape* compoundShape = u2b.convertLinkCollisions(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
|
||||
if (compoundShape)
|
||||
{
|
||||
|
||||
|
||||
btVector3 color = selectColor2();
|
||||
/* if (visual->material.get())
|
||||
/*
|
||||
if (visual->material.get())
|
||||
{
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
}
|
||||
*/
|
||||
//btVector3 localInertiaDiagonal(0, 0, 0);
|
||||
@ -182,18 +237,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
//}
|
||||
|
||||
btRigidBody* linkRigidBody = 0;
|
||||
|
||||
//btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame;
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
|
||||
// URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
|
||||
|
||||
if (!mappings.m_createMultiBody)
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
|
||||
rbci.m_startWorldTransform = inertialFrameInWorldSpace;
|
||||
// linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
|
||||
//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
|
||||
btRigidBody* body = new btRigidBody(rbci);
|
||||
btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
linkRigidBody = body;
|
||||
|
||||
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
|
||||
@ -201,13 +249,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
|
||||
|
||||
// gfxBridge.createRigidBodyGraphicsObject(body, color);
|
||||
|
||||
// linkInfo->m_bulletRigidBody = body;
|
||||
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
|
||||
} else
|
||||
{
|
||||
if (cache.m_bulletMultiBody==0)
|
||||
@ -216,74 +258,39 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
bool canSleep = false;
|
||||
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
|
||||
cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// linkInfo->m_collisionShape = compoundShape;
|
||||
// linkInfo->m_localInertiaDiagonal = localInertiaDiagonal;
|
||||
// linkInfo->m_mass = mass;
|
||||
//linkInfo->m_localVisualFrame =visual_frame;
|
||||
// linkInfo->m_localInertialFrame =inertialFrame;
|
||||
|
||||
|
||||
|
||||
// linkInfo->m_thisLink = link.get();
|
||||
// const Link* p = link.get();
|
||||
// mappings.m_link2rigidbody.insert(p, linkInfo);
|
||||
|
||||
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint)//(*link).parent_joint && pp)
|
||||
{
|
||||
if (hasParentJoint) {
|
||||
|
||||
// const Joint* pj = (*link).parent_joint.get();
|
||||
btTransform offsetInA,offsetInB;
|
||||
static bool once = true;
|
||||
|
||||
offsetInA.setIdentity();
|
||||
static bool toggle=false;
|
||||
|
||||
//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
|
||||
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
|
||||
|
||||
offsetInB.setIdentity();
|
||||
//offsetInB = visual_frame.inverse();
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
|
||||
|
||||
bool disableParentCollision = true;
|
||||
switch (jointType)
|
||||
{
|
||||
case URDF2Bullet::FixedJoint:
|
||||
{
|
||||
if (mappings.m_createMultiBody)
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
|
||||
printf("Fixed joint (btMultiBody)\n");
|
||||
//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
|
||||
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
|
||||
//toggle=!toggle;
|
||||
//mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
// rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
|
||||
|
||||
|
||||
btMatrix3x3 rm(rot);
|
||||
btScalar y,p,r;
|
||||
rm.getEulerZYX(y,p,r);
|
||||
//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
|
||||
//linkInfo->m_localVisualFrame.setIdentity();
|
||||
printf("y=%f,p=%f,r=%f\n", y,p,r);
|
||||
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
printf("Fixed joint\n");
|
||||
@ -291,12 +298,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
btMatrix3x3 rm(offsetInA.getBasis());
|
||||
btScalar y,p,r;
|
||||
rm.getEulerZYX(y,p,r);
|
||||
//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
|
||||
//linkInfo->m_localVisualFrame.setIdentity();
|
||||
printf("y=%f,p=%f,r=%f\n", y,p,r);
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
|
||||
//we could also use btFixedConstraint but it has some issues
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -305,34 +311,20 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
|
||||
// btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
|
||||
// world->addConstraint(fixed,true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDF2Bullet::ContinuousJoint:
|
||||
case URDF2Bullet::RevoluteJoint:
|
||||
{
|
||||
if (mappings.m_createMultiBody)
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
/*mappings.m_bulletMultiBody->setupRevolute(
|
||||
linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
|
||||
|
||||
parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(),
|
||||
btVector3(0,0,0),//offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
*/
|
||||
|
||||
|
||||
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
//parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
//linkInfo->m_localVisualFrame.setIdentity();
|
||||
|
||||
} else
|
||||
{
|
||||
@ -343,7 +335,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -356,7 +348,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -370,7 +362,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
|
||||
@ -387,7 +379,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
}
|
||||
case URDF2Bullet::PrismaticJoint:
|
||||
{
|
||||
if (mappings.m_createMultiBody)
|
||||
if (createMultiBody)
|
||||
{
|
||||
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
@ -399,8 +391,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
|
||||
int principleAxis = jointAxisInJointSpace.closestAxis();
|
||||
switch (principleAxis)
|
||||
@ -437,21 +428,18 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
default:
|
||||
{
|
||||
printf("Error: unsupported joint type in URDF (%d)\n", jointType);
|
||||
btAssert(0);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (mappings.m_createMultiBody)
|
||||
if (createMultiBody)
|
||||
{
|
||||
if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(cache.m_bulletMultiBody, mbLinkIndex); //or mbLinkIndex-1??? double-check
|
||||
|
||||
//btCompoundShape* comp = new btCompoundShape();
|
||||
//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
|
||||
|
||||
btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
col->setCollisionShape(compoundShape);
|
||||
@ -462,8 +450,6 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
|
||||
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
|
||||
|
||||
//tr.setOrigin(local_origin[0]);
|
||||
//tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
bool isDynamic = true;
|
||||
@ -501,7 +487,19 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2Bullet(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,mappings,pathPrefix);
|
||||
ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
|
||||
{
|
||||
URDF2BulletCachedData cache;
|
||||
|
||||
InitURDF2BulletCache(u2b,cache);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
ConvertURDF2BulletInternal(u2b, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -5,115 +5,14 @@
|
||||
#include <string>
|
||||
class btVector3;
|
||||
class btTransform;
|
||||
|
||||
struct URDF2BulletConfig
|
||||
{
|
||||
URDF2BulletConfig()
|
||||
:m_createMultiBody(true)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
//true to create a btMultiBody, false to use btRigidBody
|
||||
bool m_createMultiBody;
|
||||
};
|
||||
|
||||
struct URDF2BulletCachedData
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
:m_totalNumJoints1(0),
|
||||
m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0)
|
||||
{
|
||||
|
||||
}
|
||||
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
||||
|
||||
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
|
||||
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
||||
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
class btMultiBody* m_bulletMultiBody;
|
||||
|
||||
//this will be initialized in the constructor
|
||||
int m_totalNumJoints1;
|
||||
int getParentUrdfIndex(int linkIndex) const
|
||||
{
|
||||
return m_urdfLinkParentIndices[linkIndex];
|
||||
}
|
||||
int getMbIndexFromUrdfIndex(int urdfIndex) const
|
||||
{
|
||||
if (urdfIndex==-2)
|
||||
return -2;
|
||||
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
|
||||
}
|
||||
|
||||
|
||||
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
|
||||
{
|
||||
return m_urdfLink2rigidBodies[urdfLinkIndex];
|
||||
}
|
||||
|
||||
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
|
||||
/*
|
||||
struct URDF_LinkInformation
|
||||
{
|
||||
const Link* m_thisLink;
|
||||
int m_linkIndex;
|
||||
//int m_parentIndex;
|
||||
|
||||
btTransform m_localInertialFrame;
|
||||
//btTransform m_localVisualFrame;
|
||||
btTransform m_bodyWorldTransform;
|
||||
btVector3 m_localInertiaDiagonal;
|
||||
btScalar m_mass;
|
||||
|
||||
btCollisionShape* m_collisionShape;
|
||||
btRigidBody* m_bulletRigidBody;
|
||||
|
||||
URDF_LinkInformation()
|
||||
:m_thisLink(0),
|
||||
m_linkIndex(-2),
|
||||
//m_parentIndex(-2),
|
||||
m_collisionShape(0),
|
||||
m_bulletRigidBody(0)
|
||||
{
|
||||
|
||||
}
|
||||
virtual ~URDF_LinkInformation()
|
||||
{
|
||||
printf("~\n");
|
||||
}
|
||||
};
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
class btMultiBodyDynamicsWorld;
|
||||
class btTransform;
|
||||
|
||||
class URDF2Bullet
|
||||
{
|
||||
|
||||
|
||||
public:
|
||||
|
||||
enum {
|
||||
RevoluteJoint=1,
|
||||
PrismaticJoint,
|
||||
@ -122,8 +21,7 @@ public:
|
||||
PlanarJoint,
|
||||
FixedJoint,
|
||||
};
|
||||
void printTree(int linkIndex, int indentationLevel=0);
|
||||
|
||||
|
||||
///return >=0 for the root link index, -1 if there is no root link
|
||||
virtual int getRootLinkIndex() const = 0;
|
||||
|
||||
@ -136,23 +34,35 @@ public:
|
||||
///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<int>& childLinkIndices) const =0;
|
||||
|
||||
virtual bool getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
|
||||
|
||||
virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0;
|
||||
|
||||
virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
|
||||
///create Bullet collision shapes from URDF 'Collision' objects, specified in inertial frame of the link.
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0;
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0;
|
||||
|
||||
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
|
||||
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) const = 0;
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const =0;
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) const = 0;
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) const = 0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache);
|
||||
void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0);
|
||||
|
||||
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix);
|
||||
|
||||
|
||||
void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, const char* pathPrefix);
|
||||
|
||||
#endif //_URDF2BULLET_H
|
||||
|
||||
|
||||
@ -58,7 +58,7 @@ class btRigidBody;
|
||||
|
||||
enum RotateOrder
|
||||
{
|
||||
RO_XYZ,
|
||||
RO_XYZ=0,
|
||||
RO_XZY,
|
||||
RO_YXZ,
|
||||
RO_YZX,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user