in src/esp/physics/bullet/BulletURDFImporter.cpp [283:654]
Mn::Matrix4 BulletURDFImporter::convertURDF2BulletInternal(
int urdfLinkIndex,
const Mn::Matrix4& parentTransformInWorldSpace,
btMultiBodyDynamicsWorld* world1,
std::map<int, std::unique_ptr<btCompoundShape>>& linkCompoundShapes,
std::map<int, std::vector<std::unique_ptr<btCollisionShape>>>&
linkChildShapes,
bool recursive) {
ESP_VERY_VERBOSE() << "++++++++++++++++++++++++++++++++++++++";
ESP_VERY_VERBOSE() << "convertURDF2BulletInternal...";
ESP_VERY_VERBOSE() << " recursive = " << recursive;
Mn::Matrix4 linkTransformInWorldSpace;
ESP_VERY_VERBOSE() << "urdfLinkIndex =" << urdfLinkIndex;
int mbLinkIndex = cache->getMbIndexFromUrdfIndex(urdfLinkIndex);
ESP_VERY_VERBOSE() << "mbLinkIndex =" << mbLinkIndex;
int urdfParentIndex = cache->getParentUrdfIndex(urdfLinkIndex);
ESP_VERY_VERBOSE() << "urdfParentIndex =" << urdfParentIndex;
int mbParentIndex = cache->getMbIndexFromUrdfIndex(urdfParentIndex);
ESP_VERY_VERBOSE() << "mbParentIndex =" << mbParentIndex;
Mn::Matrix4 parentLocalInertialFrame;
btScalar parentMass(1);
Mn::Vector3 parentLocalInertiaDiagonal(1);
if (urdfParentIndex == -2) {
ESP_VERY_VERBOSE() << "root link has no parent";
} else {
getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal,
parentLocalInertialFrame);
}
ESP_VERY_VERBOSE() << "about to get mass/inertia";
btScalar mass = 0;
Mn::Matrix4 localInertialFrame;
Mn::Vector3 localInertiaDiagonal(0);
getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal,
localInertialFrame);
ESP_VERY_VERBOSE() << "about to get joint info";
Mn::Matrix4 parent2joint;
int jointType = 0;
Mn::Vector3 jointAxisInJointSpace;
btScalar jointLowerLimit = NAN;
btScalar jointUpperLimit = NAN;
btScalar jointDamping = NAN;
btScalar jointFriction = NAN;
btScalar jointMaxForce = NAN;
btScalar jointMaxVelocity = NAN;
bool hasParentJoint = getJointInfo2(
urdfLinkIndex, parent2joint, linkTransformInWorldSpace,
jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit,
jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
if ((flags & CUF_USE_MJCF) != 0) {
linkTransformInWorldSpace =
parentTransformInWorldSpace * linkTransformInWorldSpace;
} else {
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
}
ESP_VERY_VERBOSE() << "about to convert link collision shapes";
btCompoundShape* compoundShape =
convertLinkCollisionShapes(urdfLinkIndex, btTransform(localInertialFrame),
linkChildShapes[mbLinkIndex]);
ESP_VERY_VERBOSE() << "about to deal with compoundShape";
if (compoundShape) {
if (mass != 0) {
if ((flags & CUF_USE_URDF_INERTIA) == 0) {
btVector3 btLocalIntertiaDiagonal;
compoundShape->calculateLocalInertia(mass, btLocalIntertiaDiagonal);
localInertiaDiagonal = Mn::Vector3(btLocalIntertiaDiagonal);
btAssert(localInertiaDiagonal[0] < 1e10);
btAssert(localInertiaDiagonal[1] < 1e10);
btAssert(localInertiaDiagonal[2] < 1e10);
}
io::URDF::LinkContactInfo contactInfo;
getLinkContactInfo(urdfLinkIndex, contactInfo);
// temporary inertia scaling until we load inertia from URDF
if ((contactInfo.m_flags & io::URDF::CONTACT_HAS_INERTIA_SCALING) != 0) {
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
}
}
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
if (cache->m_bulletMultiBody == nullptr) {
bool isFixedBase = (mass == 0);
int totalNumJoints = cache->m_totalNumJoints1;
cache->m_bulletMultiBody =
new btMultiBody(totalNumJoints, mass, btVector3(localInertiaDiagonal),
isFixedBase, canSleep);
if ((flags & CUF_GLOBAL_VELOCITIES_MB) != 0) {
cache->m_bulletMultiBody->useGlobalVelocities(true);
}
if ((flags & CUF_USE_MJCF) != 0) {
cache->m_bulletMultiBody->setBaseWorldTransform(
btTransform(linkTransformInWorldSpace));
}
// registerMultiBody
cache->m_urdfLinkLocalInertialFrames[urdfLinkIndex] =
btTransform(localInertialFrame);
}
// create a joint if necessary
// TODO: joint friction and damping set for links are not used in Bullet
// yet. We will need to create motors to simulate these manually. See
// btMultiBodyLink header file for details.
if (hasParentJoint) {
Mn::Matrix4 offsetInA, offsetInB;
offsetInA = parentLocalInertialFrame.invertedRigid() * parent2joint;
offsetInB = localInertialFrame.invertedRigid();
Mn::Quaternion parentRotToThis = Mn::Quaternion::fromMatrix(
offsetInB.rotation() * offsetInA.invertedRigid().rotation());
bool disableParentCollision = true;
if (cache->m_bulletMultiBody) {
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping =
jointDamping;
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction =
jointFriction;
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit =
jointLowerLimit;
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit =
jointUpperLimit;
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce =
jointMaxForce;
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity =
jointMaxVelocity;
}
switch (jointType) {
case io::URDF::SphericalJoint: {
// TODO: link mapping?
// creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
cache->m_bulletMultiBody->setupSpherical(
mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex,
btQuaternion(parentRotToThis), btVector3(offsetInA.translation()),
btVector3(-offsetInB.translation()), disableParentCollision);
break;
}
case io::URDF::PlanarJoint: {
// TODO: link mapping?
// creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
cache->m_bulletMultiBody->setupPlanar(
mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex,
btQuaternion(parentRotToThis),
quatRotate(btQuaternion(
Mn::Quaternion::fromMatrix(offsetInB.rotation())),
btVector3(jointAxisInJointSpace)),
btVector3(offsetInA.translation()), disableParentCollision);
break;
}
case io::URDF::FloatingJoint:
case io::URDF::FixedJoint: {
if ((jointType == io::URDF::FloatingJoint) ||
(jointType == io::URDF::PlanarJoint)) {
printf(
"Warning: joint unsupported, creating a fixed joint instead.");
}
// TODO: link mapping?
// creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
// todo: adjust the center of mass transform and pivot axis properly
cache->m_bulletMultiBody->setupFixed(
mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex,
btQuaternion(parentRotToThis), btVector3(offsetInA.translation()),
btVector3(-offsetInB.translation()));
break;
}
case io::URDF::ContinuousJoint:
case io::URDF::RevoluteJoint: {
// TODO: link mapping?
// creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
cache->m_bulletMultiBody->setupRevolute(
mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex,
btQuaternion(parentRotToThis),
quatRotate(btQuaternion(
Mn::Quaternion::fromMatrix(offsetInB.rotation())),
btVector3(jointAxisInJointSpace)),
btVector3(offsetInA.translation()),
btVector3(-offsetInB.translation()), disableParentCollision);
if (jointType == io::URDF::RevoluteJoint &&
jointLowerLimit <= jointUpperLimit) {
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(
cache->m_bulletMultiBody, mbLinkIndex, jointLowerLimit,
jointUpperLimit);
world1->addMultiBodyConstraint(con);
cache->m_jointLimitConstraints.emplace(
mbLinkIndex,
JointLimitConstraintInfo(mbLinkIndex, jointLowerLimit,
jointUpperLimit, con));
}
break;
}
case io::URDF::PrismaticJoint: {
// TODO: link mapping?
// creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
cache->m_bulletMultiBody->setupPrismatic(
mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex,
btQuaternion(parentRotToThis),
quatRotate(btQuaternion(
Mn::Quaternion::fromMatrix(offsetInB.rotation())),
btVector3(jointAxisInJointSpace)),
btVector3(offsetInA.translation()), // parent2joint.getOrigin(),
btVector3(-offsetInB.translation()), disableParentCollision);
if (jointLowerLimit <= jointUpperLimit) {
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(
cache->m_bulletMultiBody, mbLinkIndex, jointLowerLimit,
jointUpperLimit);
world1->addMultiBodyConstraint(con);
cache->m_jointLimitConstraints.emplace(
mbLinkIndex,
JointLimitConstraintInfo(mbLinkIndex, jointLowerLimit,
jointUpperLimit, con));
}
break;
}
default: {
ESP_VERY_VERBOSE() << "Invalid joint type." btAssert(0);
}
}
}
{
btMultiBodyLinkCollider* col =
new btMultiBodyLinkCollider(cache->m_bulletMultiBody, mbLinkIndex);
col->setCollisionShape(compoundShape);
// TODO: better track the collision shapes
linkCompoundShapes[mbLinkIndex].reset(compoundShape);
Mn::Matrix4 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
ESP_VERY_VERBOSE()
<< "~~~~~~~~~~~~~ col->setWorldTransform(btTransform(tr)):" << tr;
col->setWorldTransform(btTransform(tr));
// base and fixed? -> static, otherwise flag as dynamic
bool isDynamic =
!(mbLinkIndex < 0 && cache->m_bulletMultiBody->hasFixedBase());
io::URDF::LinkContactInfo contactInfo;
getLinkContactInfo(urdfLinkIndex, contactInfo);
processContactParameters(contactInfo, col);
if (mbLinkIndex >= 0) //???? double-check +/- 1
{
// if the base is static and all joints in the chain between this link
// and the base are fixed, then this link is static too (doesn't merge
// islands)
if (cache->m_bulletMultiBody->getBaseMass() == 0) {
bool allJointsFixed = true;
int testLinkIndex = mbLinkIndex;
do {
if (cache->m_bulletMultiBody->getLink(testLinkIndex).m_jointType !=
btMultibodyLink::eFixed) {
allJointsFixed = false;
break;
}
testLinkIndex =
cache->m_bulletMultiBody->getLink(testLinkIndex).m_parent;
} while (testLinkIndex > 0);
if (allJointsFixed) {
col->setCollisionFlags(col->getCollisionFlags() |
btCollisionObject::CF_STATIC_OBJECT);
isDynamic = false;
}
}
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col;
if ((flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT) != 0) {
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_flags &=
~BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
}
if ((flags & CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) != 0) {
cache->m_bulletMultiBody->getLink(mbLinkIndex).m_flags |=
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
}
} else {
{
if (cache->m_bulletMultiBody->getBaseMass() == 0) {
col->setCollisionFlags(col->getCollisionFlags() |
btCollisionObject::CF_STATIC_OBJECT);
isDynamic = false;
}
}
cache->m_bulletMultiBody->setBaseCollider(col);
}
CORRADE_ASSERT(
isDynamic != col->isStaticOrKinematicObject(),
"Static or Kinematic object erroneously marked dynamic. This "
"should not happen.",
{});
// By convention, fixed links should be authored in the URDF as
// Noncollidable. Then we will create fixed rigid bodies, separate from
// the multibody, which will be collidable (CollisionGroup::Static) (see
// BulletArticulatedObject.cpp :: "initializeFromURDF()").
int collisionFilterGroup = int(CollisionGroup::Robot);
int colGroup = 0, colMask = 0;
int collisionFlags =
getCollisionGroupAndMask(urdfLinkIndex, colGroup, colMask);
if ((collisionFlags & io::URDF::HAS_COLLISION_GROUP) != 0) {
collisionFilterGroup = colGroup;
}
int collisionFilterMask = uint32_t(CollisionGroupHelper::getMaskForGroup(
CollisionGroup(collisionFilterGroup)));
// We don't like overriding the mask in the URDF; we disable support for this.
// We prefer to only override the group, while still using getMaskForGroup
// (above) for mask computation.
#if 0
if (collisionFlags & io::URDF::HAS_COLLISION_MASK) {
collisionFilterMask = colMask;
}
#endif
world1->addCollisionObject(col, collisionFilterGroup,
collisionFilterMask);
const auto debugModel = getModel();
std::string linkDebugName = "URDF, " + debugModel->m_name + ", link " +
debugModel->getLink(urdfLinkIndex)->m_name;
BulletCollisionHelper::get().mapCollisionObjectTo(col, linkDebugName);
}
}
if (recursive) {
ESP_VERY_VERBOSE() << "about to recurse";
std::vector<int> urdfChildIndices;
getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
int numChildren = urdfChildIndices.size();
for (int i = 0; i < numChildren; i++) {
int urdfChildLinkIndex = urdfChildIndices[i];
convertURDF2BulletInternal(urdfChildLinkIndex, linkTransformInWorldSpace,
world1, linkCompoundShapes, linkChildShapes,
recursive);
}
}
return linkTransformInWorldSpace;
}