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

Improved URDF support for btMultiBody and separate graphics/collision/inertial frames and shapes

Browse Source 
Fix WinXP GetTickCount64 with a typedef
Expose debug drawing mode/flags in UI (hot keys A,D,L,W for now, buttons later)
GLInstancingRenderer: tweak near/far planes to allow closer approach of camera
btDiscreteDynamicsWorld: enable debug drawing for btGeneric6DofSpring2Constraint
btMultiBodyDynamicsWorld: enable basic debug drawing for btMultiBody
btMultibody: allow center-of-mass shift for prismatic and fixed constraint
This commit is contained in:
erwincoumans
2015-01-22 17:56:24 -08:00
parent 6e70f285e1
commit a159fbac69
28 changed files with 1022 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files

735
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -12,9 +12,10 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr
static bool enableConstraints = true;//false;
ImportUrdfSetup::ImportUrdfSetup()
{
sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");//
sprintf(m_fileName,"r2d2.urdf");
}
ImportUrdfSetup::~ImportUrdfSetup()
@@ -86,12 +87,26 @@ struct URDF_LinkInformation
{
const Link* m_thisLink;
int m_linkIndex;
int m_parentIndex;
//int m_parentIndex;
btTransform m_localInertialFrame;
btTransform m_localVisualFrame;
//btTransform m_localVisualFrame;
btTransform m_bodyWorldTransform;
btVector3 m_localInertiaDiagonal;
btScalar m_mass;
btCollisionShape* m_collisionShape;
btRigidBody* m_bulletRigidBody;
URDF_LinkInformation()
:m_thisLink(0),
m_linkIndex(-2),
//m_parentIndex(-2),
m_collisionShape(0),
m_bulletRigidBody(0)
{
}
virtual ~URDF_LinkInformation()
{
printf("~\n");
@@ -107,17 +122,30 @@ struct URDF_JointInformation
struct URDF2BulletMappings
{
btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
//btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
btAlignedObjectArray<btTransform> m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
//btAlignedObjectArray<btTransform> m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
btAlignedObjectArray<btScalar> m_linkMasses;
btAlignedObjectArray<btVector3> m_linkLocalDiagonalInertiaTensors;
btAlignedObjectArray<btTransform> m_jointTransforms;//for root, it is identity
btAlignedObjectArray<int> m_parentIndices;//for root, it is identity
btAlignedObjectArray<btVector3> m_jointAxisArray;
btAlignedObjectArray<btTransform> m_jointOffsetInParent;
btAlignedObjectArray<btTransform> m_jointOffsetInChild;
btAlignedObjectArray<int> m_jointTypeArray;
//btAlignedObjectArray<btVector3> m_linkLocalDiagonalInertiaTensors;
//btAlignedObjectArray<int> m_parentIndices;//for root, it is identity
//btAlignedObjectArray<btVector3> m_jointAxisArray;
//btAlignedObjectArray<btTransform> m_jointOffsetInParent;
//btAlignedObjectArray<btTransform> m_jointOffsetInChild;
//btAlignedObjectArray<int> m_jointTypeArray;
bool m_createMultiBody;
int m_totalNumJoints;
btMultiBody* m_bulletMultiBody;
URDF2BulletMappings()
:m_createMultiBody(false),
m_totalNumJoints(0),
m_bulletMultiBody(0)
{
}
};
enum MyFileType
@@ -126,7 +154,8 @@ enum MyFileType
FILE_COLLADA=2
};
btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const char* pathPrefix)
template <typename T>
btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix)
{
btCollisionShape* shape = 0;
@@ -151,6 +180,8 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->initializePolyhedralFeatures();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
@@ -164,6 +195,7 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
urdf::Box* box = (urdf::Box*)visual->geometry.get();
btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
shape = boxShape;
shape ->setMargin(0.001);
break;
@@ -348,32 +380,46 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
}
return shape;
}
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
{
btCollisionShape* shape = 0;
//btCollisionShape* shape = 0;
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
btScalar mass = 1;
btScalar mass = 0;
btTransform inertialFrame;
inertialFrame.setIdentity();
const Link* parentLink = (*link).getParent();
URDF_LinkInformation* pp = 0;
int linkIndex = mappings.m_linkMasses.size();
btVector3 localInertiaDiagonal(0,0,0);
int parentIndex = -1;
if (parentLink)
{
parentIndex = parentLink->m_link_index;
btAssert(parentIndex>=0);
}
{
URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink);
if (ppRigidBody)
{
pp = (*ppRigidBody);
btRigidBody* parentRigidBody = pp->m_bulletRigidBody;
btTransform tr = parentRigidBody->getWorldTransform();
pp = (*ppRigidBody);
btTransform tr = pp->m_bodyWorldTransform;
printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z());
}
}
(*link).m_link_index = linkIndex;
if ((*link).inertial)
{
mass = (*link).inertial->mass;
localInertiaDiagonal.setValue((*link).inertial->ixx,(*link).inertial->iyy,(*link).inertial->izz);
inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z));
inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
}
@@ -394,54 +440,123 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
} else
{
linkTransformInWorldSpace = parentTransformInWorldSpace;
}
{
printf("converting visuals of link %s",link->name.c_str());
for (int v=0;v<(int)link->collision_array.size();v++)
{
const Collision* visual = link->collision_array[v].get();
shape = convertVisualToCollisionShape(visual,pathPrefix);
if (shape)
{
gfxBridge.createCollisionShapeGraphicsObject(shape);
btCompoundShape* tmpGfxShape = new btCompoundShape();
btVector3 color = selectColor();
/* if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
btVector3 localInertia(0,0,0);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
for (int v=0;v<(int)link->visual_array.size();v++)
{
const Visual* vis = link->visual_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(vis,pathPrefix);
if (childShape)
{
btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
if (!mappings.m_createMultiBody)
{
tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
} else
{
tmpGfxShape->addChildShape(childTrans,childShape);
}
}
}
btCompoundShape* compoundShape = new btCompoundShape();
for (int v=0;v<(int)link->collision_array.size();v++)
{
const Collision* col = link->collision_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
if (childShape)
{
btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
if (!mappings.m_createMultiBody)
{
compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
} else
{
compoundShape->addChildShape(childTrans,childShape);
}
}
}
if (compoundShape)
{
btVector3 color = selectColor();
/* if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
//btVector3 localInertiaDiagonal(0, 0, 0);
//if (mass)
//{
// shape->calculateLocalInertia(mass, localInertiaDiagonal);
//}
//btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame;
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame;
URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
if (!mappings.m_createMultiBody)
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
rbci.m_startWorldTransform = inertialFrameInWorldSpace;
linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
btRigidBody* body = new btRigidBody(rbci);
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
//hack-> transfer user inder from visual to collision shape
compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
gfxBridge.createRigidBodyGraphicsObject(body, color);
linkInfo->m_bulletRigidBody = body;
} else
{
if (mappings.m_bulletMultiBody==0)
{
bool multiDof = true;
bool canSleep = false;
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
int totalNumJoints = mappings.m_totalNumJoints;
mappings.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
}
}
btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
btTransform visual_frame;
visual_frame.setOrigin(visual_pos);
visual_frame.setRotation(visual_orn);
btTransform visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame;
rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass;
btRigidBody* body = new btRigidBody(rbci);
world1->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask);
// body->setFriction(0);
gfxBridge.createRigidBodyGraphicsObject(body,color);
URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
linkInfo->m_bulletRigidBody = body;
linkInfo->m_localVisualFrame =visual_frame;
linkInfo->m_collisionShape = compoundShape;
linkInfo->m_localInertiaDiagonal = localInertiaDiagonal;
linkInfo->m_mass = mass;
//linkInfo->m_localVisualFrame =visual_frame;
linkInfo->m_localInertialFrame =inertialFrame;
linkInfo->m_thisLink = link.get();
const Link* p = link.get();
@@ -452,68 +567,173 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
{
btAssert(pp);
btRigidBody* parentBody =pp->m_bulletRigidBody;
const Joint* pj = (*link).parent_joint.get();
btTransform offsetInA,offsetInB;
static bool once = true;
offsetInA.setIdentity();
offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
static bool toggle=false;
//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
offsetInA = pp->m_localInertialFrame.inverse()*parent2joint;
offsetInB.setIdentity();
offsetInB = visual_frame.inverse();
//offsetInB = visual_frame.inverse();
offsetInB = inertialFrame.inverse();
bool disableParentCollision = true;
btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
switch (pj->type)
{
case Joint::FIXED:
{
printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
if (mappings.m_createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
printf("Fixed joint (btMultiBody)\n");
//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
btQuaternion rot = parent2joint.inverse().getRotation();
//toggle=!toggle;
mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
btMatrix3x3 rm(rot);
btScalar y,p,r;
rm.getEulerZYX(y,p,r);
//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
//linkInfo->m_localVisualFrame.setIdentity();
printf("y=%f,p=%f,r=%f\n", y,p,r);
if (enableConstraints)
world1->addConstraint(dof6,true);
// btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
// world->addConstraint(fixed,true);
} else
{
printf("Fixed joint\n");
btMatrix3x3 rm(offsetInA.getBasis());
btScalar y,p,r;
rm.getEulerZYX(y,p,r);
//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
//linkInfo->m_localVisualFrame.setIdentity();
printf("y=%f,p=%f,r=%f\n", y,p,r);
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
// btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
// world->addConstraint(fixed,true);
}
break;
}
case Joint::CONTINUOUS:
case Joint::REVOLUTE:
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
if (mappings.m_createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(),
btVector3(0,0,0),//offsetInB.getOrigin(),
disableParentCollision);
dof6->setAngularLowerLimit(btVector3(0,0,1000));
dof6->setAngularUpperLimit(btVector3(0,0,-1000));
if (enableConstraints)
world1->addConstraint(dof6,true);
printf("Revolute/Continuous joint\n");
/*
mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
linkInfo->m_localVisualFrame.setIdentity();
*/
} else
{
//only handle principle axis at the moment,
//@todo(erwincoumans) orient the constraint for non-principal axis
btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
int principleAxis = axis.closestAxis();
switch (principleAxis)
{
case 0:
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_ZYX);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(-1,0,0));
dof6->setAngularLowerLimit(btVector3(1,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
break;
}
case 1:
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XZY);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,-1,0));
dof6->setAngularLowerLimit(btVector3(0,1,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
break;
}
case 2:
default:
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XYZ);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,-1));
dof6->setAngularLowerLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
}
};
printf("Revolute/Continuous joint\n");
}
break;
}
case Joint::PRISMATIC:
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
if (mappings.m_createMultiBody)
{
mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
//mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
// parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(),
// offsetInB.getOrigin(),
// disableParentCollision);
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
} else
{
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
printf("Prismatic\n");
if (enableConstraints)
world1->addConstraint(dof6,true);
printf("Prismatic\n");
}
break;
}
default:
@@ -523,10 +743,63 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
}
}
if (mappings.m_createMultiBody)
{
if (compoundShape->getNumChildShapes()>0)
{
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mappings.m_bulletMultiBody, linkIndex-1);
//btCompoundShape* comp = new btCompoundShape();
//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
col->setCollisionShape(compoundShape);
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
//tr.setOrigin(local_origin[0]);
//tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
bool isDynamic = true;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector3 color = selectColor();//(0.0,0.0,0.5);
gfxBridge.createCollisionObjectGraphicsObject(col,color);
btScalar friction = 0.5f;
col->setFriction(friction);
if (parentIndex>=0)
{
mappings.m_bulletMultiBody->getLink(linkIndex-1).m_collider=col;
} else
{
mappings.m_bulletMultiBody->setBaseCollider(col);
}
}
}
//mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
//mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
}
}
}
mappings.m_linkMasses.push_back(mass);
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
@@ -546,257 +819,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
}
btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints)
{
btScalar mass = 0.f;
btTransform localInertialTransform; localInertialTransform.setIdentity();
btVector3 localInertiaDiagonal(0,0,0);
{
if ((*link).inertial)
{
mass = (*link).inertial->mass;
btMatrix3x3 inertiaMat;
inertiaMat.setIdentity();
inertiaMat.setValue(
(*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz,
(*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz,
(*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz);
btScalar threshold = 0.00001f;
int maxSteps=20;
btMatrix3x3 inertia2PrincipalAxis;
inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps);
localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]);
btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
localInertialTransform.setOrigin(inertiaLocalCOM);
btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
btMatrix3x3 inertiaOrnMat(inertiaOrn);
if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f
|| localInertiaDiagonal[2] == 0.f))
{
b3Warning("Error: inertia should not be zero if mass is positive\n");
localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1));
localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat);
}
else
{
localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
}
}
}
btTransform linkTransformInWorldSpace;
int parentIndex = -1;
const Link* parentLink = (*link).getParent();
if (parentLink)
{
parentIndex = parentLink->m_link_index;
btAssert(parentIndex>=0);
}
int linkIndex = mappings.m_linkMasses.size();
btTransform parent2joint;
if ((*link).parent_joint)
{
const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
} else
{
linkTransformInWorldSpace = parentTransformInWorldSpace;
btAssert(mb==0);
bool multiDof = true;
bool canSleep = false;
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
}
btAssert(mb);
(*link).m_link_index = linkIndex;
//compute this links center of mass transform, aligned with the principal axis of inertia
{
//btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame;
mappings.m_linkMasses.push_back(mass);
mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
if ((*link).parent_joint)
{
btTransform offsetInA,offsetInB;
offsetInA.setIdentity();
//offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint;
offsetInA = parent2joint;
offsetInB.setIdentity();
//offsetInB = localInertialTransform.inverse();
const Joint* pj = (*link).parent_joint.get();
//btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z);
btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
mappings.m_jointAxisArray.push_back(jointAxis);
mappings.m_jointOffsetInParent.push_back(offsetInA);
mappings.m_jointOffsetInChild.push_back(offsetInB);
mappings.m_jointTypeArray.push_back(pj->type);
switch (pj->type)
{
case Joint::FIXED:
{
printf("Fixed joint\n");
mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin());
break;
}
case Joint::CONTINUOUS:
case Joint::REVOLUTE:
{
printf("Revolute joint\n");
mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true);
mb->finalizeMultiDof();
//mb->setJointVel(linkIndex-1,1);
break;
}
case Joint::PRISMATIC:
{
mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true);
printf("Prismatic joint\n");
break;
}
default:
{
printf("Unknown joint\n");
btAssert(0);
}
};
} else
{
mappings.m_jointAxisArray.push_back(btVector3(0,0,0));
btTransform ident;
ident.setIdentity();
mappings.m_jointOffsetInParent.push_back(ident);
mappings.m_jointOffsetInChild.push_back(ident);
mappings.m_jointTypeArray.push_back(-1);
}
}
//btCompoundShape* compoundShape = new btCompoundShape();
btCollisionShape* shape = 0;
for (int v=0;v<(int)link->collision_array.size();v++)
{
const Collision* visual = link->collision_array[v].get();
shape = convertVisualToCollisionShape(visual,pathPrefix);
if (shape)//childShape)
{
gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
//btVector3 color = selectColor();
/*
if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
btVector3 localInertia(0,0,0);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
//btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
btTransform visual_frame;
visual_frame.setOrigin(visual_pos);
visual_frame.setRotation(visual_orn);
btTransform childTransform;
childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform
// compoundShape->addChildShape(childTransform,childShape);
}
}
if (shape)//compoundShape->getNumChildShapes()>0)
{
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
col->setCollisionShape(shape);
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
//tr.setOrigin(local_origin[0]);
//tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
bool isDynamic = true;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector3 color = selectColor();//(0.0,0.0,0.5);
gfxBridge.createCollisionObjectGraphicsObject(col,color);
btScalar friction = 0.5f;
col->setFriction(friction);
if (parentIndex>=0)
{
mb->getLink(linkIndex-1).m_collider=col;
} else
{
mb->setBaseCollider(col);
}
}
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints);
}
else
{
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
return mb;
}
void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
@@ -874,27 +896,22 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
int numJoints = (*robot).m_numJoints;
static bool useFeatherstone = false;
if (!useFeatherstone)
{
URDF2BulletMappings mappings;
mappings.m_createMultiBody = useFeatherstone;
mappings.m_totalNumJoints = numJoints;
URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
if (useFeatherstone)
{
btMultiBody* mb = mappings.m_bulletMultiBody;
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
}
}
//the btMultiBody support is work-in-progress :-)
#if 1
else
{
URDF2BulletMappings mappings;
btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints);
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
//m_dynamicsWorld->integrateTransforms(0.f);
}
#endif//
useFeatherstone = !useFeatherstone;
printf("numJoints/DOFS = %d\n", numJoints);
@@ -923,7 +940,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2.5;
groundOrigin[upAxis]=-1.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
@@ -932,7 +949,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}
void ImportUrdfSetup::stepSimulation(float deltaTime)