From 1a4ce475f761737a72517f008d6dcd40f200f7e7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 5 Jun 2015 11:46:53 -0700 Subject: [PATCH] fix an issue with btMultiBodyConstraint, automatically 'finalizeMultiDof' to pre-allocate jacobian data enable joint limit for slider/prismatic joint in btMultiBody version of URDF loader --- .../ImportURDFDemo/ImportURDFSetup.cpp | 64 +++--- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 182 +++++++++--------- .../OpenCL/rigidbody/GpuRigidBodyDemo.cpp | 54 +++--- .../Featherstone/btMultiBodyConstraint.cpp | 46 +++-- .../Featherstone/btMultiBodyConstraint.h | 57 +++--- .../Featherstone/btMultiBodyDynamicsWorld.h | 25 ++- .../btMultiBodyJointLimitConstraint.cpp | 35 ++-- .../btMultiBodyJointLimitConstraint.h | 8 +- .../Featherstone/btMultiBodyJointMotor.cpp | 59 +++--- .../Featherstone/btMultiBodyJointMotor.h | 9 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 7 +- .../Featherstone/btMultiBodyPoint2Point.h | 10 +- 12 files changed, 307 insertions(+), 249 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 7ffda4b72..fbcde7d9b 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -29,17 +29,17 @@ class ImportUrdfSetup : public CommonMultiBodyBase { char m_fileName[1024]; - + struct ImportUrdfInternalData* m_data; bool m_useMultiBody; - + public: ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName); virtual ~ImportUrdfSetup(); virtual void initPhysics(); virtual void stepSimulation(float deltaTime); - + void setFileName(const char* urdfFileName); virtual void resetCamera() @@ -64,7 +64,7 @@ struct ImportUrdfInternalData :m_numMotors(0) { } - + btScalar m_motorTargetVelocities[MAX_NUM_MOTORS]; btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS]; int m_numMotors; @@ -93,10 +93,10 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, gFileNameArray.clear(); gFileNameArray.push_back("r2d2.urdf"); - - + + //load additional urdf file names from file - + FILE* f = fopen("urdf_files.txt","r"); if (f) { @@ -112,10 +112,10 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, gFileNameArray.push_back(fileName); } } while (result==1); - + fclose(f); } - + int numFileNames = gFileNameArray.size(); if (count>=numFileNames) @@ -173,14 +173,14 @@ void ImportUrdfSetup::initPhysics() +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); - + btVector3 gravity(0,0,0); gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); - - + + //now print the tree using the new interface MyURDFImporter u2b(m_guiHelper); bool loadOk = u2b.loadURDF(m_fileName); @@ -188,17 +188,17 @@ void ImportUrdfSetup::initPhysics() if (loadOk) { u2b.printTree(); - + btTransform identityTrans; identityTrans.setIdentity(); - - + + { - - + + btMultiBody* mb = 0; - - + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user int rootLinkIndex = u2b.getRootLinkIndex(); printf("urdf root link index = %d\n",rootLinkIndex); @@ -209,22 +209,22 @@ void ImportUrdfSetup::initPhysics() if (m_useMultiBody) { - - - + + + //create motors for each joint - + for (int i=0;igetNumLinks();i++) { int mbLinkIndex = i; - if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute + if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic ) { if (m_data->m_numMotorsm_numMotors++; } } - + } } } - + //the btMultiBody support is work-in-progress :-) - - + for (int i=0;igetNumMultiBodyConstraints();i++) + { + m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof(); + } + + bool createGround=true; if (createGround) @@ -284,7 +288,7 @@ void ImportUrdfSetup::stepSimulation(float deltaTime) { m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]); } - + //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); } diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 87adbe87c..92a0b24db 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -25,7 +25,7 @@ static btVector4 colors[4] = static btVector3 selectColor2() { - + static int curColor = 0; btVector4 color = colors[curColor]; curColor++; @@ -37,9 +37,9 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL { btAlignedObjectArray childIndices; u2b.getLinkChildIndices(linkIndex,childIndices); - + int numChildren = childIndices.size(); - + indentationLevel+=2; int count = 0; for (int i=0;i m_urdfLinkParentIndices; btAlignedObjectArray m_urdfLinkIndices2BulletLinkIndices; btAlignedObjectArray m_urdfLink2rigidBodies; btAlignedObjectArray m_urdfLinkLocalInertialFrames; - + int m_currentMultiBodyLinkIndex; - + class btMultiBody* m_bulletMultiBody; - + //this will be initialized in the constructor int m_totalNumJoints1; int getParentUrdfIndex(int linkIndex) const @@ -86,26 +86,26 @@ struct URDF2BulletCachedData return -2; return m_urdfLinkIndices2BulletLinkIndices[urdfIndex]; } - - + + void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) { m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; } - + class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex) { return m_urdfLink2rigidBodies[urdfLinkIndex]; } - + void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) { btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0); - + m_urdfLink2rigidBodies[urdfLinkIndex] = body; m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; } - + }; void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex) @@ -129,7 +129,7 @@ void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedDat { cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex; cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++; - + btAlignedObjectArray childIndices; u2b.getLinkChildIndices(urdfLinkIndex,childIndices); for (int i=0;imaterial.get()) { color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); @@ -239,19 +239,19 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat //{ // shape->calculateLocalInertia(mass, localInertiaDiagonal); //} - + btRigidBody* linkRigidBody = 0; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame; - + if (!createMultiBody) { btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); linkRigidBody = body; - + world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); - + compoundShape->setUserIndex(graphicsIndex); - + creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } else @@ -263,19 +263,19 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); - + cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } - + } - + //create a joint if necessary if (hasParentJoint) { - + btTransform offsetInA,offsetInB; offsetInA = parentLocalInertialFrame.inverse()*parent2joint; offsetInB = localInertialFrame.inverse(); - + bool disableParentCollision = true; switch (jointType) { @@ -284,32 +284,32 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly - + printf("Fixed joint (btMultiBody)\n"); btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - - + + } else { printf("Fixed joint\n"); - + btMatrix3x3 rm(offsetInA.getBasis()); btScalar y,p,r; rm.getEulerZYX(y,p,r); printf("y=%f,p=%f,r=%f\n", y,p,r); - + //we could also use btFixedConstraint but it has some issues btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); 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); } @@ -320,14 +320,14 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { if (createMultiBody) { - - + + cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - + } else { //only handle principle axis at the moment, @@ -340,10 +340,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, 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; @@ -353,10 +353,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, 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; @@ -367,10 +367,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, 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); } @@ -383,17 +383,17 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { if (createMultiBody) { - + cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); - + creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); - //world1->addMultiBodyConstraint(con); - printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); - + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); + world1->addMultiBodyConstraint(con); + //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); + } else { btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); @@ -420,12 +420,12 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit)); } }; - + dof6->setAngularLowerLimit(btVector3(0,0,0)); dof6->setAngularUpperLimit(btVector3(0,0,0)); if (enableConstraints) world1->addConstraint(dof6,true); - + printf("Prismatic\n"); } break; @@ -436,41 +436,41 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btAssert(0); } } - + } - + if (createMultiBody) { if (compoundShape->getNumChildShapes()>0) { btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); - + compoundShape->setUserIndex(graphicsIndex); - + 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 + //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 - + 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 = selectColor2();//(0.0,0.0,0.5); - + creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); - + btScalar friction = 0.5f; - + col->setFriction(friction); - + if (mbLinkIndex>=0) //???? double-check +/- 1 { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col; @@ -481,20 +481,20 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } } } - - + + btAlignedObjectArray urdfChildIndices; u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices); - + int numChildren = urdfChildIndices.size(); - + for (int i=0;igetAppInterface()->m_window; m_data = new GpuRigidBodyDemoInternalData; } GpuRigidBodyDemo::~GpuRigidBodyDemo() { - + delete m_data; } @@ -97,9 +97,9 @@ static void PairKeyboardCallback(int key, int state) { if (key=='R' && state) { - gReset = true; + //gReset = true; } - + //b3DefaultKeyboardCallback(key,state); oldCallback(key,state); } @@ -122,13 +122,13 @@ void GpuRigidBodyDemo::initPhysics() cl_program rbProg=0; m_data->m_copyTransformsToVBOKernel = b3OpenCLUtils::compileCLKernelFromString(m_clData->m_clContext,m_clData->m_clDevice,s_rigidBodyKernelString,"copyTransformsToVBOKernel",&errNum,rbProg); - + m_data->m_config.m_maxConvexBodies = b3Max(m_data->m_config.m_maxConvexBodies,gGpuArraySizeX*gGpuArraySizeY*gGpuArraySizeZ+10); m_data->m_config.m_maxConvexShapes = m_data->m_config.m_maxConvexBodies; int maxPairsPerBody = 16; m_data->m_config.m_maxBroadphasePairs = maxPairsPerBody*m_data->m_config.m_maxConvexBodies; m_data->m_config.m_maxContactCapacity = m_data->m_config.m_maxBroadphasePairs; - + b3GpuNarrowPhase* np = new b3GpuNarrowPhase(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue,m_data->m_config); b3GpuBroadphaseInterface* bp =0; @@ -155,11 +155,11 @@ void GpuRigidBodyDemo::initPhysics() } - + m_guiHelper->getRenderInterface()->writeTransforms(); - - + + } @@ -171,8 +171,8 @@ void GpuRigidBodyDemo::exitPhysics() delete m_data->m_rigidBodyPipeline; delete m_data->m_broadphaseDbvt; - - + + delete m_data->m_np; m_data->m_np = 0; delete m_data->m_bp; @@ -185,7 +185,7 @@ void GpuRigidBodyDemo::exitPhysics() void GpuRigidBodyDemo::renderScene() { m_guiHelper->getRenderInterface()->renderScene(); - + } @@ -216,7 +216,7 @@ void GpuRigidBodyDemo::stepSimulation(float deltaTime) if (animate && numObjects) { B3_PROFILE("gl2cl"); - + if (!m_data->m_instancePosOrnColor) { GLuint vbo = m_instancingRenderer->getInternalData()->m_vbo; @@ -234,11 +234,11 @@ void GpuRigidBodyDemo::stepSimulation(float deltaTime) assert(err==GL_NO_ERROR); } } - + { B3_PROFILE("stepSimulation"); m_data->m_rigidBodyPipeline->stepSimulation(1./60.f); - + } if (numObjects) @@ -247,7 +247,7 @@ void GpuRigidBodyDemo::stepSimulation(float deltaTime) { b3GpuNarrowPhaseInternalData* npData = m_data->m_np->getInternalData(); npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU); - + b3AlignedObjectArray vboCPU; m_data->m_instancePosOrnColor->copyToHost(vboCPU); @@ -301,7 +301,7 @@ b3Vector3 GpuRigidBodyDemo::getRayTo(int x,int y) { if (!m_instancingRenderer) return b3MakeVector3(0,0,0); - + float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; @@ -339,7 +339,7 @@ b3Vector3 GpuRigidBodyDemo::getRayTo(int x,int y) float height = m_instancingRenderer->getScreenHeight(); aspect = width / height; - + hor*=aspect; @@ -362,7 +362,7 @@ unsigned char* GpuRigidBodyDemo::loadImage(const char* fileName, int& width, int bool GpuRigidBodyDemo::keyboardCallback(int key, int state) { - + if (m_data) { if (key==B3G_ALT ) @@ -429,17 +429,17 @@ bool GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float m_data->m_rigidBodyPipeline->castRays(rays,hitResults); if (hitResults[0].m_hitFraction<1.f) { - + int hitBodyA = hitResults[0].m_hitBody; if (m_data->m_np->getBodiesCpu()[hitBodyA].m_invMass) { //printf("hit!\n"); m_data->m_np->readbackAllBodiesToCpu(); m_data->m_pickBody = hitBodyA; - - - + + + //pivotInA b3Vector3 pivotInB; pivotInB.setInterpolate3(ray.m_from,ray.m_to,hitResults[0].m_hitFraction); @@ -458,14 +458,14 @@ bool GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float m_data->m_pickFixedBody = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0,pos,orn,fixedSphere,0,false); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); m_data->m_bp->writeAabbsToGpu(); - + if (m_data->m_pickGraphicsShapeIndex<0) { int strideInBytes = 9*sizeof(float); int numVertices = sizeof(point_sphere_vertices)/strideInBytes; int numIndices = sizeof(point_sphere_indices)/sizeof(int); m_data->m_pickGraphicsShapeIndex = m_guiHelper->getRenderInterface()->registerShape(&point_sphere_vertices[0],numVertices,point_sphere_indices,numIndices,B3_GL_POINTS); - + float color[4] ={1,0,0,1}; float scaling[4]={1,1,1,1}; @@ -480,7 +480,7 @@ bool GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float m_instancingRenderer->writeSingleInstanceTransformToGPU(pivotInB,orn,m_data->m_pickGraphicsShapeInstance); m_data->m_np->setObjectTransformCpu(pos,orn,m_data->m_pickFixedBody); } - + } pivotInB.w = 0.f; m_data->m_pickPivotInA = pivotInA; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 2cf0ff8d8..dd96ac4d8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -3,7 +3,7 @@ #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) - + btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) :m_bodyA(bodyA), m_bodyB(bodyB), @@ -13,14 +13,15 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod m_jacSizeA(0), m_jacSizeBoth(0), m_isUnilateral(isUnilateral), - m_maxAppliedImpulse(100) + m_maxAppliedImpulse(100), + m_numDofsFinalized(-1) { - finalizeMultiDof(); + } -void btMultiBodyConstraint::finalizeMultiDof() +void btMultiBodyConstraint::updateJacobianSizes() { - if(m_bodyA) + if(m_bodyA) { if(m_bodyA->isMultiDof()) m_jacSizeA = (6 + m_bodyA->getNumDofs()); @@ -37,7 +38,12 @@ void btMultiBodyConstraint::finalizeMultiDof() } else m_jacSizeBoth = m_jacSizeA; - +} + +void btMultiBodyConstraint::allocateJacobiansMultiDof() +{ + updateJacobianSizes(); + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); m_data.resize((2 + m_jacSizeBoth) * m_numRows); } @@ -48,25 +54,27 @@ btMultiBodyConstraint::~btMultiBodyConstraint() void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { - for (int i = 0; i < ndof; ++i) + for (int i = 0; i < ndof; ++i) data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } -btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, +btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, btScalar* jacOrgA, btScalar* jacOrgB, const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, + const btVector3& posAworld, const btVector3& posBworld, btScalar posError, const btContactSolverInfo& infoGlobal, btScalar lowerLimit, btScalar upperLimit, btScalar relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { + + solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; + solverConstraint.m_linkB = m_linkB; btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; @@ -181,13 +189,13 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } else if(rb1) { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormalOnB; } { - + btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; @@ -233,7 +241,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); } - + // btScalar d = denom0+denom1; if (d>SIMD_EPSILON) @@ -244,10 +252,10 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr { //disable the constraint row to handle singularity/redundant constraint solverConstraint.m_jacDiagABInv = 0.f; - } + } } - + //compute rhs and remaining solverConstraint fields btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; @@ -260,7 +268,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr { ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) + for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; } else if(rb0) @@ -271,7 +279,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr { ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) + for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; } @@ -321,11 +329,11 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; - { + { btScalar positionalError = 0.f; btScalar velocityError = desiredVelocity - rel_vel;// * damping; - + btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 2a96ea85b..2c78373f5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -54,7 +54,7 @@ protected: int m_posOffset; bool m_isUnilateral; - + int m_numDofsFinalized; btScalar m_maxAppliedImpulse; @@ -66,11 +66,11 @@ protected: void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, btScalar* jacOrgA, btScalar* jacOrgB, const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, + const btVector3& posAworld, const btVector3& posBworld, btScalar posError, const btContactSolverInfo& infoGlobal, btScalar lowerLimit, btScalar upperLimit, @@ -82,11 +82,14 @@ public: btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); virtual ~btMultiBodyConstraint(); - void finalizeMultiDof(); + void updateJacobianSizes(); + void allocateJacobiansMultiDof(); + + virtual void finalizeMultiDof()=0; virtual int getIslandIdA() const =0; virtual int getIslandIdB() const =0; - + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)=0; @@ -108,17 +111,17 @@ public: // current constraint position // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral // NOTE: ignored position for friction rows. - btScalar getPosition(int row) const - { - return m_data[m_posOffset + row]; + btScalar getPosition(int row) const + { + return m_data[m_posOffset + row]; } - void setPosition(int row, btScalar pos) - { - m_data[m_posOffset + row] = pos; + void setPosition(int row, btScalar pos) + { + m_data[m_posOffset + row] = pos; } - + bool isUnilateral() const { return m_isUnilateral; @@ -127,21 +130,21 @@ public: // jacobian blocks. // each of size 6 + num_links. (jacobian2 is null if no body2.) // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. - btScalar* jacobianA(int row) - { - return &m_data[m_numRows + row * m_jacSizeBoth]; + btScalar* jacobianA(int row) + { + return &m_data[m_numRows + row * m_jacSizeBoth]; } - const btScalar* jacobianA(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth)]; + const btScalar* jacobianA(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth)]; } - btScalar* jacobianB(int row) - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; + btScalar* jacobianB(int row) + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } - const btScalar* jacobianB(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; + const btScalar* jacobianB(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } btScalar getMaxAppliedImpulse() const @@ -152,7 +155,7 @@ public: { m_maxAppliedImpulse = maxImp; } - + virtual void debugDraw(class btIDebugDraw* drawer)=0; }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index f3a9e320b..dd0f7711f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -38,11 +38,11 @@ protected: virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); virtual void solveConstraints(btContactSolverInfo& solverInfo); - + public: btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - + virtual ~btMultiBodyDynamicsWorld (); virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); @@ -51,12 +51,27 @@ public: virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + virtual int getNumMultiBodyConstraints() const + { + return m_multiBodyConstraints.size(); + } + + virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) + { + return m_multiBodyConstraints[constraintIndex]; + } + + virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const + { + return m_multiBodyConstraints[constraintIndex]; + } + virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); virtual void debugDrawWorld(); - + virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 778594b40..c984d41e0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -26,14 +26,18 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo :btMultiBodyConstraint(body,body,link,link,2,true), m_lowerBound(lower), m_upperBound(upper) +{ + +} + +void btMultiBodyJointLimitConstraint::finalizeMultiDof() { // the data.m_jacobians never change, so may as well // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); + allocateJacobiansMultiDof(); + + unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA); // row 0: the lower bound jacobianA(0)[offset] = 1; @@ -41,7 +45,10 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo //jacobianA(1)[offset] = -1; jacobianB(1)[offset] = -1; + + m_numDofsFinalized = m_jacSizeBoth; } + btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() { } @@ -87,7 +94,13 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint { // only positions need to be updated -- data.m_jacobians and force // directions were set in the ctor and never change. - + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + // row 0: the lower bound setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent @@ -101,7 +114,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint constraintRow.m_multiBodyB = m_bodyB; const btScalar posError = 0; //why assume it's zero? const btVector3 dummy(0, 0, 0); - + btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); { btScalar penetration = getPosition(row); @@ -139,7 +152,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint } } - - - + + + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index c20780bf2..55b8d122b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -30,6 +30,8 @@ public: btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); virtual ~btMultiBodyJointLimitConstraint(); + virtual void finalizeMultiDof(); + virtual int getIslandIdA() const; virtual int getIslandIdB() const; @@ -41,7 +43,7 @@ public: { //todo(erwincoumans) } - + }; #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 29d505ebe..b08509567 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -23,44 +23,41 @@ subject to the following restrictions: btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) :btMultiBodyConstraint(body,body,link,link,1,true), - m_desiredVelocity(desiredVelocity) + m_desiredVelocity(desiredVelocity) { - int linkDoF = 0; + m_maxAppliedImpulse = maxMotorImpulse; // the data.m_jacobians never change, so may as well // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link); - // row 0: the lower bound - // row 0: the lower bound - jacobianA(0)[offset] = 1; } + +void btMultiBodyJointMotor::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA); + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; + + m_numDofsFinalized = m_jacSizeBoth; +} btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), :btMultiBodyConstraint(body,body,link,link,1,true), - m_desiredVelocity(desiredVelocity) + m_desiredVelocity(desiredVelocity) { btAssert(linkDoF < body->getLink(link).m_dofCount); m_maxAppliedImpulse = maxMotorImpulse; - // the data.m_jacobians never change, so may as well - // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link); - - // row 0: the lower bound - // row 0: the lower bound - jacobianA(0)[offset] = 1; } btMultiBodyJointMotor::~btMultiBodyJointMotor() { @@ -101,18 +98,26 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con { // only positions need to be updated -- data.m_jacobians and force // directions were set in the ctor and never change. - - + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + const btScalar posError = 0; const btVector3 dummy(0, 0, 0); for (int row=0;row