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

Further improvements in Bullet MuJoCo MJCF import:

Browse Source 
[MJCF import] improved humanoid.xml, inverted_pendulum.xml import (automatically add dummy-links to connect multiple joints between bodies)
[MJCF import] fix bug in quaternion conversion (w,x,y,z) -> (x,y,z,w)
This commit is contained in:
Erwin Coumans
2017-01-15 11:22:24 -08:00
parent 42ef8f538e
commit 310fabcd7e
9 changed files with 451 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -231,6 +231,8 @@ void ConvertURDF2BulletInternal(
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
std::string linkName = u2b.getLinkName(urdfLinkIndex);
if (flags & CUF_USE_SDF)
{
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
@@ -314,7 +316,11 @@ void ConvertURDF2BulletInternal(
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_USE_MJCF)
{
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
}
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
}
@@ -443,6 +449,16 @@ void ConvertURDF2BulletInternal(
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int colGroup=0, colMask=0;
int flags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
if (flags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
}
if (flags & URDF_HAS_COLLISION_MASK)
{
collisionFilterMask = colMask;
}
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector4 color = selectColor2();//(0.0,0.0,0.5);
@@ -505,7 +521,12 @@ void ConvertURDF2Bullet(
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
if (flags & CUF_USE_MJCF)
{
} else
{
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);