Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -4,7 +4,6 @@
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
|
||||
@@ -20,131 +19,122 @@
|
||||
static bool enableConstraints = true;
|
||||
|
||||
static btVector4 colors[4] =
|
||||
{
|
||||
btVector4(1,0,0,1),
|
||||
btVector4(0,1,0,1),
|
||||
btVector4(0,1,1,1),
|
||||
btVector4(1,1,0,1),
|
||||
{
|
||||
btVector4(1, 0, 0, 1),
|
||||
btVector4(0, 1, 0, 1),
|
||||
btVector4(0, 1, 1, 1),
|
||||
btVector4(1, 1, 0, 1),
|
||||
};
|
||||
|
||||
|
||||
static btVector4 selectColor2()
|
||||
{
|
||||
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
return color;
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor &= 3;
|
||||
return color;
|
||||
}
|
||||
|
||||
|
||||
|
||||
struct URDF2BulletCachedData
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
:
|
||||
m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0),
|
||||
m_totalNumJoints1(0)
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
: m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0),
|
||||
m_totalNumJoints1(0)
|
||||
{
|
||||
}
|
||||
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
||||
|
||||
}
|
||||
//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;
|
||||
|
||||
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
|
||||
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
class btMultiBody* m_bulletMultiBody;
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
//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 btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
|
||||
{
|
||||
return m_urdfLink2rigidBodies[urdfLinkIndex];
|
||||
}
|
||||
|
||||
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* 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 btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
void registerRigidBody(int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex] == 0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
};
|
||||
|
||||
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
|
||||
//for (int i=0;i<childIndices.size();i++)
|
||||
//{
|
||||
// b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
|
||||
//}
|
||||
cache.m_totalNumJoints1 += childIndices.size();
|
||||
for (int i=0;i<childIndices.size();i++)
|
||||
{
|
||||
int childIndex =childIndices[i];
|
||||
ComputeTotalNumberOfJoints(u2b,cache,childIndex);
|
||||
}
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex, childIndices);
|
||||
//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
|
||||
//for (int i=0;i<childIndices.size();i++)
|
||||
//{
|
||||
// b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
|
||||
//}
|
||||
cache.m_totalNumJoints1 += childIndices.size();
|
||||
for (int i = 0; i < childIndices.size(); i++)
|
||||
{
|
||||
int childIndex = childIndices[i];
|
||||
ComputeTotalNumberOfJoints(u2b, cache, childIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
||||
{
|
||||
cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
|
||||
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
|
||||
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
|
||||
for (int i=0;i<childIndices.size();i++)
|
||||
{
|
||||
ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
|
||||
}
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
|
||||
for (int i = 0; i < childIndices.size(); i++)
|
||||
{
|
||||
ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
|
||||
{
|
||||
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
||||
cache.m_totalNumJoints1 = 0;
|
||||
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
||||
cache.m_totalNumJoints1 = 0;
|
||||
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
if (rootLinkIndex>=0)
|
||||
{
|
||||
ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
|
||||
int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
if (rootLinkIndex >= 0)
|
||||
{
|
||||
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
|
||||
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
|
||||
|
||||
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
|
||||
|
||||
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
|
||||
ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
|
||||
}
|
||||
|
||||
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
|
||||
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
|
||||
}
|
||||
}
|
||||
|
||||
void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col)
|
||||
@@ -176,106 +166,96 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar tmpUrdfScaling=2;
|
||||
|
||||
btScalar tmpUrdfScaling = 2;
|
||||
|
||||
void ConvertURDF2BulletInternal(
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
URDF2BulletCachedData& cache, int urdfLinkIndex,
|
||||
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix,
|
||||
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn=0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut=0)
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
URDF2BulletCachedData& cache, int urdfLinkIndex,
|
||||
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix,
|
||||
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut = 0)
|
||||
{
|
||||
B3_PROFILE("ConvertURDF2BulletInternal2");
|
||||
//b3Printf("start converting/extracting data from URDF interface\n");
|
||||
//b3Printf("start converting/extracting data from URDF interface\n");
|
||||
|
||||
btTransform linkTransformInWorldSpace;
|
||||
linkTransformInWorldSpace.setIdentity();
|
||||
btTransform linkTransformInWorldSpace;
|
||||
linkTransformInWorldSpace.setIdentity();
|
||||
|
||||
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
|
||||
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
||||
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
||||
btRigidBody* parentRigidBody = 0;
|
||||
|
||||
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
||||
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
||||
btRigidBody* parentRigidBody = 0;
|
||||
|
||||
//b3Printf("mb link index = %d\n",mbLinkIndex);
|
||||
//b3Printf("mb link index = %d\n",mbLinkIndex);
|
||||
|
||||
btTransform parentLocalInertialFrame;
|
||||
parentLocalInertialFrame.setIdentity();
|
||||
btScalar parentMass(1);
|
||||
btVector3 parentLocalInertiaDiagonal(1,1,1);
|
||||
btVector3 parentLocalInertiaDiagonal(1, 1, 1);
|
||||
|
||||
if (urdfParentIndex==-2)
|
||||
{
|
||||
//b3Printf("root link has no parent\n");
|
||||
} else
|
||||
{
|
||||
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
|
||||
//b3Printf("mb parent index = %d\n",mbParentIndex);
|
||||
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
||||
u2b.getMassAndInertia2(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame, flags);
|
||||
if (urdfParentIndex == -2)
|
||||
{
|
||||
//b3Printf("root link has no parent\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
|
||||
//b3Printf("mb parent index = %d\n",mbParentIndex);
|
||||
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
||||
u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
|
||||
}
|
||||
|
||||
}
|
||||
btScalar mass = 0;
|
||||
btTransform localInertialFrame;
|
||||
localInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0, 0, 0);
|
||||
u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
|
||||
|
||||
btScalar mass = 0;
|
||||
btTransform localInertialFrame;
|
||||
localInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0,0,0);
|
||||
u2b.getMassAndInertia2(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame, flags);
|
||||
btTransform parent2joint;
|
||||
parent2joint.setIdentity();
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btTransform parent2joint;
|
||||
parent2joint.setIdentity();
|
||||
|
||||
int jointType;
|
||||
btVector3 jointAxisInJointSpace;
|
||||
btScalar jointLowerLimit;
|
||||
btScalar jointUpperLimit;
|
||||
btScalar jointDamping;
|
||||
btScalar jointFriction;
|
||||
int jointType;
|
||||
btVector3 jointAxisInJointSpace;
|
||||
btScalar jointLowerLimit;
|
||||
btScalar jointUpperLimit;
|
||||
btScalar jointDamping;
|
||||
btScalar jointFriction;
|
||||
btScalar jointMaxForce;
|
||||
btScalar jointMaxVelocity;
|
||||
|
||||
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
|
||||
std::string linkName = u2b.getLinkName(urdfLinkIndex);
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
{
|
||||
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
{
|
||||
parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (flags & CUF_USE_MJCF)
|
||||
{
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*linkTransformInWorldSpace;
|
||||
} else
|
||||
{
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
else
|
||||
{
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
|
||||
}
|
||||
}
|
||||
|
||||
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
|
||||
btCollisionShape* compoundShape = tmpShape;
|
||||
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity())
|
||||
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0) == btTransform::getIdentity())
|
||||
{
|
||||
compoundShape = tmpShape->getChildShape(0);
|
||||
}
|
||||
|
||||
|
||||
int graphicsIndex;
|
||||
{
|
||||
B3_PROFILE("convertLinkVisualShapes");
|
||||
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex+1))
|
||||
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
|
||||
{
|
||||
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex+1];
|
||||
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
|
||||
UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
|
||||
u2b.setLinkColor2(urdfLinkIndex, matColor);
|
||||
}
|
||||
@@ -291,85 +271,77 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (compoundShape)
|
||||
{
|
||||
|
||||
|
||||
if (compoundShape)
|
||||
{
|
||||
UrdfMaterialColor matColor;
|
||||
btVector4 color2 = selectColor2();
|
||||
btVector3 specular(0.5,0.5,0.5);
|
||||
if (u2b.getLinkColor2(urdfLinkIndex,matColor))
|
||||
btVector4 color2 = selectColor2();
|
||||
btVector3 specular(0.5, 0.5, 0.5);
|
||||
if (u2b.getLinkColor2(urdfLinkIndex, matColor))
|
||||
{
|
||||
color2 = matColor.m_rgbaColor;
|
||||
specular = matColor.m_specularColor;
|
||||
}
|
||||
|
||||
/*
|
||||
/*
|
||||
if (visual->material.get())
|
||||
{
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
}
|
||||
*/
|
||||
if (mass)
|
||||
{
|
||||
if (!(flags & CUF_USE_URDF_INERTIA))
|
||||
{
|
||||
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
|
||||
btAssert(localInertiaDiagonal[0] < 1e10);
|
||||
btAssert(localInertiaDiagonal[1] < 1e10);
|
||||
btAssert(localInertiaDiagonal[2] < 1e10);
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||
//temporary inertia scaling until we load inertia from URDF
|
||||
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
|
||||
{
|
||||
localInertiaDiagonal*=contactInfo.m_inertiaScaling;
|
||||
}
|
||||
}
|
||||
if (mass)
|
||||
{
|
||||
if (!(flags & CUF_USE_URDF_INERTIA))
|
||||
{
|
||||
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
|
||||
btAssert(localInertiaDiagonal[0] < 1e10);
|
||||
btAssert(localInertiaDiagonal[1] < 1e10);
|
||||
btAssert(localInertiaDiagonal[2] < 1e10);
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
//temporary inertia scaling until we load inertia from URDF
|
||||
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
|
||||
{
|
||||
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
|
||||
}
|
||||
}
|
||||
|
||||
btRigidBody* linkRigidBody = 0;
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
|
||||
bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
|
||||
btRigidBody* linkRigidBody = 0;
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
|
||||
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
|
||||
if (!canSleep)
|
||||
{
|
||||
body->forceActivationState(DISABLE_DEACTIVATION);
|
||||
}
|
||||
|
||||
linkRigidBody = body;
|
||||
linkRigidBody = body;
|
||||
|
||||
world1->addRigidBody(body);
|
||||
world1->addRigidBody(body);
|
||||
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
|
||||
processContactParameters(contactInfo, body);
|
||||
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2,specular, graphicsIndex);
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
|
||||
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
|
||||
|
||||
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
} else
|
||||
{
|
||||
if (cache.m_bulletMultiBody==0)
|
||||
{
|
||||
|
||||
|
||||
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
|
||||
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cache.m_bulletMultiBody == 0)
|
||||
{
|
||||
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
|
||||
if (flags & CUF_GLOBAL_VELOCITIES_MB)
|
||||
{
|
||||
cache.m_bulletMultiBody->useGlobalVelocities(true);
|
||||
@@ -378,21 +350,20 @@ void ConvertURDF2BulletInternal(
|
||||
{
|
||||
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
|
||||
}
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
|
||||
}
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
}
|
||||
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint) {
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint)
|
||||
{
|
||||
btTransform offsetInA, offsetInB;
|
||||
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
||||
|
||||
btTransform offsetInA,offsetInB;
|
||||
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
||||
|
||||
bool disableParentCollision = true;
|
||||
bool disableParentCollision = true;
|
||||
|
||||
if (createMultiBody && cache.m_bulletMultiBody)
|
||||
{
|
||||
@@ -404,103 +375,108 @@ void ConvertURDF2BulletInternal(
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
|
||||
}
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
switch (jointType)
|
||||
{
|
||||
case URDFFloatingJoint:
|
||||
case URDFPlanarJoint:
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
if ((jointType==URDFFloatingJoint)||(jointType==URDFPlanarJoint))
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
|
||||
{
|
||||
printf("Warning: joint unsupported, creating a fixed joint instead.");
|
||||
}
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin());
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("Fixed joint\n");
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
|
||||
|
||||
} else
|
||||
{
|
||||
//b3Printf("Fixed joint\n");
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
|
||||
//backward compatibility
|
||||
if (flags & CUF_RESERVED )
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
} else
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
}
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
world1->addMultiBodyConstraint(con);
|
||||
}
|
||||
} else
|
||||
{
|
||||
else
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
|
||||
}
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
world1->addMultiBodyConstraint(con);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//backwards compatibility
|
||||
if (flags & CUF_RESERVED )
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
|
||||
} else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
|
||||
}
|
||||
} else
|
||||
{
|
||||
//disable joint limits
|
||||
if (flags & CUF_RESERVED )
|
||||
else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,1,-1);
|
||||
} else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,1,-1);
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//disable joint limits
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, 1, -1);
|
||||
}
|
||||
else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, 1, -1);
|
||||
}
|
||||
}
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
//b3Printf("Revolute/Continuous joint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
//b3Printf("Revolute/Continuous joint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
if (jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
@@ -509,38 +485,36 @@ void ConvertURDF2BulletInternal(
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
world1->addMultiBodyConstraint(con);
|
||||
}
|
||||
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
|
||||
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
|
||||
}
|
||||
else
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
|
||||
//b3Printf("Prismatic\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
|
||||
//b3Printf("Prismatic\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
if (createMultiBody)
|
||||
{
|
||||
//if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
col->setCollisionShape(compoundShape);
|
||||
col->setCollisionShape(compoundShape);
|
||||
|
||||
if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
@@ -551,21 +525,21 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
}
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr = linkTransformInWorldSpace;
|
||||
//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
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr = linkTransformInWorldSpace;
|
||||
//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
|
||||
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
//base and fixed? -> static, otherwise flag as dynamic
|
||||
bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
|
||||
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
|
||||
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody->hasFixedBase()) ? false : true;
|
||||
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
|
||||
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
int colGroup=0, colMask=0;
|
||||
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
|
||||
int colGroup = 0, colMask = 0;
|
||||
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, colGroup, colMask);
|
||||
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
|
||||
{
|
||||
collisionFilterGroup = colGroup;
|
||||
@@ -574,12 +548,12 @@ void ConvertURDF2BulletInternal(
|
||||
{
|
||||
collisionFilterMask = colMask;
|
||||
}
|
||||
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
|
||||
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
|
||||
|
||||
btVector4 color2 = selectColor2();//(0.0,0.0,0.5);
|
||||
btVector3 specularColor(1,1,1);
|
||||
btVector4 color2 = selectColor2(); //(0.0,0.0,0.5);
|
||||
btVector3 specularColor(1, 1, 1);
|
||||
UrdfMaterialColor matCol;
|
||||
if (u2b.getLinkColor2(urdfLinkIndex,matCol))
|
||||
if (u2b.getLinkColor2(urdfLinkIndex, matCol))
|
||||
{
|
||||
color2 = matCol.m_rgbaColor;
|
||||
specularColor = matCol.m_specularColor;
|
||||
@@ -593,75 +567,72 @@ void ConvertURDF2BulletInternal(
|
||||
u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
|
||||
processContactParameters(contactInfo, col);
|
||||
|
||||
if (mbLinkIndex>=0) //???? double-check +/- 1
|
||||
{
|
||||
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
|
||||
if (flags&CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
|
||||
if (mbLinkIndex >= 0) //???? double-check +/- 1
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col;
|
||||
if (flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags &= ~BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
||||
}
|
||||
if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
|
||||
if (flags & CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
|
||||
}
|
||||
} else
|
||||
{
|
||||
// if (canSleep)
|
||||
}
|
||||
else
|
||||
{
|
||||
// if (canSleep)
|
||||
{
|
||||
if (cache.m_bulletMultiBody->getBaseMass()==0)
|
||||
//&& cache.m_bulletMultiBody->getNumDofs()==0)
|
||||
if (cache.m_bulletMultiBody->getBaseMass() == 0)
|
||||
//&& cache.m_bulletMultiBody->getNumDofs()==0)
|
||||
{
|
||||
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
|
||||
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
cache.m_bulletMultiBody->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
|
||||
cache.m_bulletMultiBody->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
//u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
|
||||
u2b.convertLinkVisualShapes2(-1,urdfLinkIndex,pathPrefix,localInertialFrame,linkRigidBody,u2b.getBodyUniqueId());
|
||||
}
|
||||
}
|
||||
u2b.convertLinkVisualShapes2(-1, urdfLinkIndex, pathPrefix, localInertialFrame, linkRigidBody, u2b.getBodyUniqueId());
|
||||
}
|
||||
}
|
||||
|
||||
btAlignedObjectArray<int> urdfChildIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
|
||||
|
||||
btAlignedObjectArray<int> urdfChildIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
|
||||
int numChildren = urdfChildIndices.size();
|
||||
|
||||
int numChildren = urdfChildIndices.size();
|
||||
|
||||
for (int i=0;i<numChildren;i++)
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
|
||||
}
|
||||
for (int i = 0; i < numChildren; i++)
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
|
||||
}
|
||||
}
|
||||
void ConvertURDF2Bullet(
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
const btTransform& rootTransformInWorldSpace,
|
||||
btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
|
||||
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();
|
||||
InitURDF2BulletCache(u2b, cache);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
B3_PROFILE("ConvertURDF2Bullet");
|
||||
|
||||
|
||||
UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
|
||||
|
||||
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapes, &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;
|
||||
@@ -672,25 +643,24 @@ void ConvertURDF2Bullet(
|
||||
B3_PROFILE("Post process");
|
||||
btMultiBody* mb = cache.m_bulletMultiBody;
|
||||
|
||||
mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0);
|
||||
|
||||
mb->setHasSelfCollision((flags & CUF_USE_SELF_COLLISION) != 0);
|
||||
|
||||
mb->finalizeMultiDof();
|
||||
|
||||
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
|
||||
|
||||
if (flags & CUF_USE_MJCF)
|
||||
{
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
|
||||
mb->setBaseWorldTransform(rootTransformInWorldSpace * localInertialFrameRoot);
|
||||
}
|
||||
btAlignedObjectArray<btQuaternion> scratch_q;
|
||||
btAlignedObjectArray<btVector3> scratch_m;
|
||||
mb->forwardKinematics(scratch_q,scratch_m);
|
||||
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
|
||||
|
||||
mb->forwardKinematics(scratch_q, scratch_m);
|
||||
mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
|
||||
|
||||
world1->addMultiBody(mb);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user