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

Merge pull request #767 from ludi1001/urdf-joint-transforms

Browse Source 
Fix joint orientations when loading URDF files.
This commit is contained in:
erwincoumans
2016-09-02 08:21:02 -07:00
committed by GitHub
parent 23f7293a25 9d2d286c46
commit 21c60c66ec
1 changed files with 6 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -286,6 +286,7 @@ void ConvertURDF2BulletInternal(
btTransform offsetInA,offsetInB; btTransform offsetInA,offsetInB;
offsetInA = parentLocalInertialFrame.inverse()*parent2joint; offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
offsetInB = localInertialFrame.inverse(); offsetInB = localInertialFrame.inverse();
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
bool disableParentCollision = true; bool disableParentCollision = true;
switch (jointType) switch (jointType)
@@ -295,14 +296,9 @@ void ConvertURDF2BulletInternal(
if (createMultiBody) if (createMultiBody)
{ {
//todo: adjust the center of mass transform and pivot axis properly //todo: adjust the center of mass transform and pivot axis properly
//b3Printf("Fixed joint (btMultiBody)\n");
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin()); parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else } else
{ {
//b3Printf("Fixed joint\n"); //b3Printf("Fixed joint\n");
@@ -319,14 +315,12 @@ void ConvertURDF2BulletInternal(
{ {
if (createMultiBody) if (createMultiBody)
{ {
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(), -offsetInB.getOrigin(),
disableParentCollision); disableParentCollision);
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else } else
@@ -344,12 +338,10 @@ void ConvertURDF2BulletInternal(
{ {
if (createMultiBody) if (createMultiBody)
{ {
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(), -offsetInB.getOrigin(),
disableParentCollision); disableParentCollision);
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con); world1->addMultiBodyConstraint(con);