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

26
Demos3/AllBullet2Demos/main.cpp
View File

@@ -234,9 +234,10 @@ extern bool useShadowMap;
static bool visualWireframe=false;
static bool renderVisualGeometry=true;
static bool renderGrid = true;
static bool pauseSimulation=false;//true;
int gDebugDrawFlags = 0;
static bool pauseSimulation=true;
int midiBaseIndex = 176;
extern bool gDisableDeactivation;
//#include <float.h>
//unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM);
@@ -268,9 +269,28 @@ void MyKeyboardCallback(int key, int state)
//if (handled)
// return;
if (key=='a' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawAabb;
}
if (key=='c' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawContactPoints;
}
if (key == 'd' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation;
gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
}
if (key=='l' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraintLimits;
}
if (key=='w' && state)
{
visualWireframe=!visualWireframe;
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawWireframe;
}
if (key=='v' && state)
{
@@ -792,7 +812,7 @@ int main(int argc, char* argv[])
}
{
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
sCurrentDemo->physicsDebugDraw();
sCurrentDemo->physicsDebugDraw(gDebugDrawFlags);
}
}

2
Demos3/FiniteElementMethod/FiniteElementDemo.cpp
View File

@@ -317,7 +317,7 @@ void FiniteElementDemo::renderScene()
m_app->m_renderer->renderScene();
}
void FiniteElementDemo::physicsDebugDraw()
void FiniteElementDemo::physicsDebugDraw(int debugDrawFlags)
{
{
btAlignedObjectArray<btVector3FloatData> m_linePoints;

2
Demos3/FiniteElementMethod/FiniteElementDemo.h
View File

@@ -50,7 +50,7 @@ public:
virtual void renderScene();
virtual void physicsDebugDraw();
virtual void physicsDebugDraw(int debugDrawFlags);
virtual bool mouseMoveCallback(float x,float y);
virtual bool mouseButtonCallback(int button, int state, float x, float y);
virtual bool keyboardCallback(int key, int state);

2
Demos3/Geometry/DistributePoints.h
View File

@@ -181,7 +181,7 @@ public:
}
virtual void physicsDebugDraw()
virtual void physicsDebugDraw(int debugDrawFlags)
{
int lineWidth = 1;

2
Demos3/Geometry/SphereCreation.h
View File

@@ -168,7 +168,7 @@ public:
m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
virtual void physicsDebugDraw(int debugDrawFlags)
{
int lineWidth = 1;
int pointSize = 2;

667
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();
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());
{
btCompoundShape* tmpGfxShape = new btCompoundShape();
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* visual = link->collision_array[v].get();
shape = convertVisualToCollisionShape(visual,pathPrefix);
if (shape)
const Collision* col = link->collision_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
if (childShape)
{
gfxBridge.createCollisionShapeGraphicsObject(shape);
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())
/* 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 localInertiaDiagonal(0, 0, 0);
//if (mass)
//{
// shape->calculateLocalInertia(mass, localInertiaDiagonal);
//}
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);
//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;
linkInfo->m_localVisualFrame =visual_frame;
} 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);
}
}
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,24 +567,61 @@ 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();
static bool toggle=false;
//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
offsetInA = pp->m_localInertialFrame.inverse()*parent2joint;
offsetInA = pp->m_localVisualFrame.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:
{
if (mappings.m_createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
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);
} else
{
printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
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));
@@ -479,31 +631,98 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
if (enableConstraints)
world1->addConstraint(dof6,true);
// btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
// 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);
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);
/*
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->setAngularLowerLimit(btVector3(0,0,1000));
dof6->setAngularUpperLimit(btVector3(0,0,-1000));
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);
//mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
// parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(),
// offsetInB.getOrigin(),
// disableParentCollision);
} 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));
@@ -514,6 +733,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
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);
}
//the btMultiBody support is work-in-progress :-)
#if 1
else
if (useFeatherstone)
{
URDF2BulletMappings mappings;
btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints);
btMultiBody* mb = mappings.m_bulletMultiBody;
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
//m_dynamicsWorld->integrateTransforms(0.f);
}
#endif//
}
//the btMultiBody support is work-in-progress :-)
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)

166
Demos3/ImportURDFDemo/urdf_samples.h
View File

@@ -402,7 +402,7 @@ const char* urdf_char_r2d2 = MSTRINGIFY(
const char* urdf_char = MSTRINGIFY(
<?xml version="1.0"?>
<robot name="flexible">
<robot name="physics">
<link name="base_link">
<visual>
<geometry>
@@ -412,6 +412,15 @@ const char* urdf_char = MSTRINGIFY(
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="right_leg">
@@ -424,6 +433,17 @@ const char* urdf_char = MSTRINGIFY(
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</collision>
<inertial>
<mass value="10"/>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="base_to_right_leg" type="fixed">
@@ -439,6 +459,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_base_joint" type="fixed">
@@ -456,6 +485,15 @@ const char* urdf_char = MSTRINGIFY(
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_front_wheel_joint" type="continuous">
@@ -474,6 +512,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_back_wheel_joint" type="continuous">
@@ -493,6 +540,17 @@ const char* urdf_char = MSTRINGIFY(
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</inertial>
</link>
<joint name="base_to_left_leg" type="fixed">
@@ -508,6 +566,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_base_joint" type="fixed">
@@ -523,6 +590,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_front_wheel_joint" type="continuous">
@@ -541,6 +617,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_back_wheel_joint" type="continuous">
@@ -568,6 +653,16 @@ const char* urdf_char = MSTRINGIFY(
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_gripper_joint" type="revolute">
@@ -582,9 +677,19 @@ const char* urdf_char = MSTRINGIFY(
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
@@ -596,9 +701,19 @@ const char* urdf_char = MSTRINGIFY(
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
@@ -613,9 +728,19 @@ const char* urdf_char = MSTRINGIFY(
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
@@ -627,9 +752,19 @@ const char* urdf_char = MSTRINGIFY(
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="head">
@@ -639,6 +774,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="head_swivel" type="continuous">
@@ -655,6 +799,15 @@ const char* urdf_char = MSTRINGIFY(
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="tobox" type="fixed">
@@ -663,7 +816,8 @@ const char* urdf_char = MSTRINGIFY(
<origin xyz="0 0.1414 0.1414"/>
</joint>
</robot>
</robot>
);

2
Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h
View File

@@ -89,7 +89,7 @@ public:
}
}
virtual void physicsDebugDraw()
virtual void physicsDebugDraw(int debugDrawFlags)
{
btVector3 xUnit(1,0,0);

2
Demos3/bullet2/CollisionDetection/SupportFuncDemo.h
View File

@@ -74,7 +74,7 @@ public:
m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
virtual void physicsDebugDraw(int debugDrawFlags)
{
int width=3;
btVector3 from(0,0,0);

3
Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
View File

@@ -612,10 +612,11 @@ void FeatherstoneDemo1::renderScene()
m_glApp->m_renderer->renderScene();
}
void FeatherstoneDemo1::physicsDebugDraw()
void FeatherstoneDemo1::physicsDebugDraw(int debugDrawFlags)
{
if (m_dynamicsWorld)
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
m_dynamicsWorld->debugDrawWorld();
}
}

2
Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h
View File

@@ -89,7 +89,7 @@ public:
virtual void initPhysics();
virtual void exitPhysics();
virtual void renderScene();
virtual void physicsDebugDraw();
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void stepSimulation(float deltaTime);
};

8
Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp
View File

@@ -413,10 +413,16 @@ void LuaPhysicsSetup::stepSimulation(float deltaTime)
}
void LuaPhysicsSetup::debugDraw()
void LuaPhysicsSetup::debugDraw(int debugDrawFlags)
{
if (m_dynamicsWorld)
{
if (m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
}
m_dynamicsWorld->debugDrawWorld();
}
}

2
Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h
View File

@@ -25,7 +25,7 @@ struct LuaPhysicsSetup : public CommonPhysicsSetup
virtual void stepSimulation(float deltaTime);
virtual void debugDraw();
virtual void debugDraw(int debugDrawFlags);
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);

4
btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp
View File

@@ -226,9 +226,9 @@ void Bullet2RigidBodyDemo::renderScene()
}
void Bullet2RigidBodyDemo::physicsDebugDraw()
void Bullet2RigidBodyDemo::physicsDebugDraw(int debugDrawFlags)
{
m_physicsSetup->debugDraw();
m_physicsSetup->debugDraw(debugDrawFlags);
}
Bullet2RigidBodyDemo::~Bullet2RigidBodyDemo()

2
btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h
View File

@@ -25,7 +25,7 @@ public:
virtual void initPhysics();
virtual void exitPhysics();
virtual void renderScene();
virtual void physicsDebugDraw();
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void stepSimulation(float dt);
virtual CommonPhysicsSetup* getPhysicsSetup()
{

5
btgui/Bullet3AppSupport/BulletDemoInterface.h
View File

@@ -3,6 +3,7 @@
struct CommonGraphicsApp;
class BulletDemoInterface
{
public:
@@ -17,7 +18,7 @@ public:
virtual void exitPhysics()=0;
virtual void stepSimulation(float deltaTime)=0;
virtual void renderScene()=0;
virtual void physicsDebugDraw()=0;
virtual void physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h
virtual bool mouseMoveCallback(float x,float y)=0;
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
virtual bool keyboardCallback(int key, int state)=0;
@@ -44,7 +45,7 @@ public:
virtual void renderScene()
{
}
virtual void physicsDebugDraw()
virtual void physicsDebugDraw(int debugFlags)
{
}
virtual bool mouseMoveCallback(float x,float y)

6
btgui/Bullet3AppSupport/CommonMultiBodySetup.h
View File

@@ -125,10 +125,14 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
}
}
virtual void debugDraw()
virtual void debugDraw(int debugDrawFlags)
{
if (m_dynamicsWorld)
{
if (m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
}
m_dynamicsWorld->debugDrawWorld();
}

2
btgui/Bullet3AppSupport/CommonPhysicsSetup.h
View File

@@ -57,7 +57,7 @@ public:
virtual void stepSimulation(float deltaTime)=0;
virtual void debugDraw()=0;
virtual void debugDraw(int debugDrawFlags)=0;
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) = 0;
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;

6
btgui/Bullet3AppSupport/CommonRigidBodySetup.h
View File

@@ -118,10 +118,14 @@ struct CommonRigidBodySetup : public CommonPhysicsSetup
}
}
virtual void debugDraw()
virtual void debugDraw(int debugDrawFlags)
{
if (m_dynamicsWorld)
{
if (m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
}
m_dynamicsWorld->debugDrawWorld();
}

4
btgui/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -1091,8 +1091,8 @@ void GLInstancingRenderer::updateCamera(int upAxis)
};
float m_frustumZNear=1;
float m_frustumZFar=10000.f;
float m_frustumZNear=0.1;
float m_frustumZFar=1000.f;
// m_azi=m_azi+0.01;

52
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -34,6 +34,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
@@ -1306,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
}
}
break;
///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
case D6_SPRING_2_CONSTRAINT_TYPE:
{
{
btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if (drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
btVector3 up = tr.getBasis().getColumn(2);
btVector3 axis = tr.getBasis().getColumn(0);
btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
btScalar cy = btCos(ay);
btScalar sy = btSin(ay);
btScalar cz = btCos(az);
btScalar sz = btSin(az);
btVector3 ref;
ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
ref[1] = -sz*axis[0] + cz*axis[1];
ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
tr = p6DOF->getCalculatedTransformB();
btVector3 normal = -tr.getBasis().getColumn(0);
btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
if (minFi > maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
}
else if (minFi < maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
}
tr = p6DOF->getCalculatedTransformA();
btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
}
}
break;
}
case SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -151,7 +151,7 @@ public:
virtual void removeCollisionObject(btCollisionObject* collisionObject);
void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld();

41
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -137,16 +137,18 @@ void btMultiBody::setupFixed(int i,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_dVector = thisPivotToThisComOffset;
m_links[i].m_eVector = parentComToThisPivotOffset;
m_links[i].m_jointType = btMultibodyLink::eFixed;
m_links[i].m_dofCount = 0;
@@ -156,6 +158,7 @@ void btMultiBody::setupFixed(int i,
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
m_links[i].updateCacheMultiDof();
//
//if(m_isMultiDof)
// resizeInternalMultiDofBuffers();
@@ -180,7 +183,7 @@ void btMultiBody::setupPrismatic(int i,
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].setAxisTop(0, 0., 0., 0.);
@@ -223,7 +226,7 @@ void btMultiBody::setupRevolute(int i,
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].setAxisTop(0, jointAxis);
@@ -265,7 +268,7 @@ void btMultiBody::setupSpherical(int i,
m_posVarCnt += 4;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector = thisPivotToThisComOffset;
@@ -307,7 +310,7 @@ void btMultiBody::setupPlanar(int i,
m_posVarCnt += 3;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector.setZero();
@@ -365,7 +368,7 @@ btScalar btMultiBody::getLinkMass(int i) const
const btVector3 & btMultiBody::getLinkInertia(int i) const
{
return m_links[i].m_inertia;
return m_links[i].m_inertiaLocal;
}
btScalar btMultiBody::getJointPos(int i) const
@@ -506,7 +509,7 @@ btScalar btMultiBody::getKineticEnergy() const
for (int i = 0; i < num_links; ++i) {
result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]);
result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
}
return 0.5f * result;
@@ -526,7 +529,7 @@ btVector3 btMultiBody::getAngularMomentum() const
for (int i = 0; i < num_links; ++i) {
rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1])));
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
}
return result;
@@ -1241,7 +1244,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
// calculate Ihat_i^A
@@ -1252,9 +1255,9 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass),
//
btMatrix3x3(m_links[i].m_inertia[0], 0, 0,
0, m_links[i].m_inertia[1], 0,
0, 0, m_links[i].m_inertia[2])
btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2])
);
//
//p += vhat x Ihat vhat - done in a simpler way
@@ -1694,19 +1697,19 @@ void btMultiBody::stepVelocities(btScalar dt,
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
0, m_links[i].m_inertia[1], 0,
0, 0, m_links[i].m_inertia[2]);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2]);
}

3
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -61,7 +61,8 @@ public:
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);

80
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
@@ -755,3 +755,81 @@ void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
{
m_multiBodyConstraints.remove(constraint);
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
int nLinks = bod->getNumLinks();
///base + num m_links
world_to_local.resize(nLinks + 1);
local_origin.resize(nLinks + 1);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
{
btVector3 posr = local_origin[0];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
for (int k = 0; k<bod->getNumLinks(); k++)
{
const int parent = bod->getParent(k);
world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
}
for (int m = 0; m<bod->getNumLinks(); m++)
{
int link = m;
int index = link + 1;
btVector3 posr = local_origin[index];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
}
}
}
btDiscreteDynamicsWorld::debugDrawWorld();
}

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -54,5 +54,7 @@ public:
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

12
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -355,7 +355,7 @@ struct btMultibodyLink
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertia; // inertia of link (local frame; diagonal)
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
@@ -440,15 +440,15 @@ struct btMultibodyLink
btMultibodyLink()
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(1, 0, 0, 0),
m_cachedRotParentToThis(1, 0, 0, 0),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid)
{
m_inertia.setValue(1, 1, 1);
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
@@ -493,7 +493,7 @@ struct btMultibodyLink
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0);
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
@@ -516,7 +516,7 @@ struct btMultibodyLink
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}

5
src/LinearMath/btQuickprof.cpp
View File

@@ -43,6 +43,11 @@ static btClock gProfileClock;
#include <Xtl.h>
#else //_XBOX
#include <windows.h>
#if WINVER < 0x0600
ULONGLONG GetTickCount64() { return GetTickCount(); }
#endif
#endif //_XBOX
#include <time.h>