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,flags);
if (world1 && cache.m_bulletMultiBody)
{
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
world1->addMultiBody(mb);
}
}
void ConvertURDF2Bullet(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
B3_PROFILE("ConvertURDF2Bullet");
UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
{
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
}
if (world1 && cache.m_bulletMultiBody)
{
B3_PROFILE("Post process");
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0);
mb->finalizeMultiDof();
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
if (flags & CUF_USE_MJCF)
{
} else
{
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
world1->addMultiBody(mb);
}
}
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
if (world1 && cache.m_bulletMultiBody)
{
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
mb->setBaseWorldTransform(rootTransformInWorldSpace);
world1->addMultiBody(mb);
}
}
void ConvertURDF2Bullet(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b, cache, flags);
int urdfLinkIndex = u2b.getRootLinkIndex();
int rootIndex = u2b.getRootLinkIndex();
B3_PROFILE("ConvertURDF2Bullet");
UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER)==0;
if (recursive)
{
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
}
else
{
btAlignedObjectArray<btTransform> parentTransforms;
if (urdfLinkIndex >= parentTransforms.size())
{
parentTransforms.resize(urdfLinkIndex + 1);
}
parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
btAlignedObjectArray<childParentIndex> allIndices;
GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
allIndices.quickSort(MyIntCompareFunc);
for (int i = 0; i < allIndices.size(); i++)
{
int urdfLinkIndex = allIndices[i].m_index;
int parentIndex = allIndices[i].m_parentIndex;
btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr , world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
if ((urdfLinkIndex+1) >= parentTransforms.size())
{
parentTransforms.resize(urdfLinkIndex + 1);
}
parentTransforms[urdfLinkIndex] = tr;
}
}
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
{
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
}
if (world1 && cache.m_bulletMultiBody)
{
B3_PROFILE("Post process");
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision((flags & CUF_USE_SELF_COLLISION) != 0);
mb->finalizeMultiDof();
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
if (flags & CUF_USE_MJCF)
{
}
else
{
mb->setBaseWorldTransform(rootTransformInWorldSpace * localInertialFrameRoot);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q, scratch_m);
mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
world1->addMultiBody(mb);
}
}
