diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index a2ed9d562..3219f8294 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -118,14 +118,19 @@ struct CommonMultiBodyBase : public CommonExampleInterface m_collisionShapes.clear(); delete m_dynamicsWorld; + m_dynamicsWorld = 0; delete m_solver; + m_solver=0; delete m_broadphase; + m_broadphase=0; delete m_dispatcher; + m_dispatcher=0; delete m_collisionConfiguration; + m_collisionConfiguration=0; } virtual void syncPhysicsToGraphics() diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e6f84bab3..45065f447 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -40,6 +40,8 @@ #include "../Tutorial/Tutorial.h" #include "../Tutorial/Dof6ConstraintTutorial.h" #include "../MultiThreading/MultiThreadingExample.h" +//#include "../InverseDynamics/InverseDynamicsExample.h" + #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" #endif @@ -108,6 +110,9 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), +// ExampleEntry(0,"Inverse Dynamics"), +// ExampleEntry(1,"Inverse Dynamics URDF", "write some explanation", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), +// ExampleEntry(1,"Inverse Dynamics Prog", "write some explanation2", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY), ExampleEntry(0,"Tutorial"), ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index d1503e05f..249b0e636 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -65,6 +65,8 @@ "../MultiThreading/b3PosixThreadSupport.cpp", "../MultiThreading/b3Win32ThreadSupport.cpp", "../MultiThreading/b3ThreadSupportInterface.cpp", + "../InverseDynamics/InverseDynamicsExample.cpp", + "../InverseDynamics/InverseDynamicsExample.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", "../Collision/*", diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index b19146022..7334fbd90 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -21,6 +21,7 @@ struct GenericConstraintUserInfo class MyMultiBodyCreator : public MultiBodyCreationInterface { +protected: btMultiBody* m_bulletMultiBody; diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp new file mode 100644 index 000000000..8b67ccfba --- /dev/null +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -0,0 +1,167 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. 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, +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "InverseDynamicsExample.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "Bullet3Common/b3FileUtils.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../../Utils/b3ResourcePath.h" +#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +#include "btBulletDynamicsCommon.h" + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + +//those are static global to make it easy for the GUI to tweak them +static btScalar radius(0.2); +static btScalar kp = 100; +static btScalar kd = 20; +static btScalar maxForce = 100; + +class InverseDynamicsExample : public CommonMultiBodyBase +{ + btInverseDynamicsExampleOptions m_option; + btMultiBody* m_multiBody; +public: + InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option); + virtual ~InverseDynamicsExample(); + + virtual void initPhysics(); + virtual void stepSimulation(float deltaTime); + + void setFileName(const char* urdfFileName); + + virtual void resetCamera() + { + float dist = 3.5; + float pitch = -136; + float yaw = 28; + float targetPos[3]={0.47,0,-0.64}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } +}; + +InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option) + :CommonMultiBodyBase(helper), + m_option(option), + m_multiBody(0) +{ +} + +InverseDynamicsExample::~InverseDynamicsExample() +{ + +} + +//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. +btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans); + +void InverseDynamicsExample::initPhysics() +{ + //roboticists like Z up + int upAxis = 2; + m_guiHelper->setUpAxis(upAxis); + + createEmptyDynamicsWorld(); + btVector3 gravity(0,0,0); + gravity[upAxis]=-9.8; + m_dynamicsWorld->setGravity(gravity); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + SliderParams slider("Kp",&kp); + slider.m_minVal=-200; + slider.m_maxVal=200; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Kd",&kd); + slider.m_minVal=-50; + slider.m_maxVal=50; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("max force",&maxForce); + slider.m_minVal=0; + slider.m_maxVal=100; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + switch (m_option) + { + case BT_ID_LOAD_URDF: + { + BulletURDFImporter u2b(m_guiHelper); + bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf"); + if (loadOk) + { + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); + btTransform identityTrans; + identityTrans.setIdentity(); + ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); + m_multiBody = creation.getBulletMultiBody(); + if (m_multiBody) + { + //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator + //temporarily set some extreme damping factors until we have some joint control or constraints + m_multiBody->setAngularDamping(0.99); + m_multiBody->setLinearDamping(0.99); + b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex())); + } + } + break; + } + case BT_ID_PROGRAMMATICALLY: + { + btTransform baseWorldTrans; + baseWorldTrans.setIdentity(); + m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); + break; + } + default: + { + b3Error("Unknown option in InverseDynamicsExample::initPhysics"); + b3Assert(0); + } + }; +} + + +void InverseDynamicsExample::stepSimulation(float deltaTime) +{ + if (m_dynamicsWorld) + { + m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + } +} + +CommonExampleInterface* InverseDynamicsExampleCreateFunc(CommonExampleOptions& options) +{ + return new InverseDynamicsExample(options.m_guiHelper, btInverseDynamicsExampleOptions(options.m_option)); +} + + + diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/InverseDynamics/InverseDynamicsExample.h new file mode 100644 index 000000000..13c37a490 --- /dev/null +++ b/examples/InverseDynamics/InverseDynamicsExample.h @@ -0,0 +1,28 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. 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, +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef INVERSE_DYNAMICS_EXAMPLE_H +#define INVERSE_DYNAMICS_EXAMPLE_H + +enum btInverseDynamicsExampleOptions +{ + BT_ID_LOAD_URDF=1, + BT_ID_PROGRAMMATICALLY=2 +}; + +class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //INVERSE_DYNAMICS_EXAMPLE_H diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index 371065199..9c58e115a 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -55,7 +55,262 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl() extern bool gJointFeedbackInWorldSpace; extern bool gJointFeedbackInJointFrame; +btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans) +{ + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + int curColor = 0; + bool fixedBase = true; + bool damping = false; + bool gyro = false; + int numLinks = 2; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.04, 0.35, 0.08); + + + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 0.f; + + if(baseMass) + { + //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + shape->calculateLocalInertia(baseMass, baseInertiaDiag); + delete shape; + } + + + btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); + + + + pMultiBody->setBaseWorldTransform(baseWorldTrans); + btVector3 vel(0, 0, 0); +// pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 1.f * SIMD_PI/ 180.f; + btQuaternion quat0(btVector3(1, 0, 0).normalized(), q0); + quat0.normalize(); + ///// + + for(int i = 0; i < numLinks; ++i) + { + float linkMass = 1.f; + //if (i==3 || i==2) + // linkMass= 1000; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* shape = 0; + if (i==0) + { + shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// + } else + { + shape = new btSphereShape(radius); + } + shape->calculateLocalInertia(linkMass, linkInertiaDiag); + delete shape; + + + if(!spherical) + { + //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + + if (i==0) + { + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, + btQuaternion(0.f, 0.f, 0.f, 1.f), + hingeJointAxis, + parentComToCurrentPivot, + currentPivotToCurrentCom, false); + } else + { + btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + + pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, + btQuaternion(0.f, 0.f, 0.f, 1.f), + parentComToCurrentPivot, + currentPivotToCurrentCom); + } + + //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); + + } + else + { + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + } + } + + pMultiBody->finalizeMultiDof(); + + + + /// + world->addMultiBody(pMultiBody); + btMultiBody* mbC = pMultiBody; + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if(!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + }else + { mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + // + + + ////////////////////////////////////////////// + if(numLinks > 0) + { + btScalar q0 = 180.f * SIMD_PI/ 180.f; + if(!spherical) + { + mbC->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + double friction = 1; + { + + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]); + guiHelper->createCollisionShapeGraphicsObject(shape); + + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(shape); + + btTransform tr; + tr.setIdentity(); +//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]); + btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); + + tr.setRotation(orn); + col->setWorldTransform(tr); + + bool isDynamic = (baseMass > 0 && !fixedBase); + 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);//, 2,1+2); + + btVector3 color(0.0,0.0,0.5); + guiHelper->createCollisionObjectGraphicsObject(col,color); + +// col->setFriction(friction); + pMultiBody->setBaseCollider(col); + + } + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + + btVector3 posr = local_origin[i+1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + btCollisionShape* shape =0; + + if (i==0) + { + shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); + } else + { + + shape = new btSphereShape(radius); + } + + guiHelper->createCollisionShapeGraphicsObject(shape); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(shape); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + // col->setFriction(friction); + bool isDynamic = 1;//(linkMass > 0); + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + //if (i==0||i>numLinks-2) + { + world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + guiHelper->createCollisionObjectGraphicsObject(col,color); + + + pMultiBody->getLink(i).m_collider=col; + } + + } + + return pMultiBody; +} void InvertedPendulumPDControl::initPhysics() { @@ -87,19 +342,7 @@ void InvertedPendulumPDControl::initPhysics() m_guiHelper->setUpAxis(upAxis); - btVector4 colors[4] = - { - btVector4(1,0,0,1), - btVector4(0,1,0,1), - btVector4(0,1,1,1), - btVector4(1,1,0,1), - }; - int curColor = 0; - - - - this->createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( @@ -110,262 +353,20 @@ void InvertedPendulumPDControl::initPhysics() );//+btIDebugDraw::DBG_DrawConstraintLimits); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + btTransform baseWorldTrans; + baseWorldTrans.setIdentity(); + baseWorldTrans.setOrigin(btVector3(1,2,3)); + m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); - { - bool fixedBase = true; - bool damping = false; - bool gyro = false; - int numLinks = 2; - bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals - bool canSleep = false; - bool selfCollide = false; - btVector3 linkHalfExtents(0.05, 0.37, 0.1); - btVector3 baseHalfExtents(0.04, 0.35, 0.08); - - btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); - //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm - //init the base - btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 0.f; - - if(baseMass) - { - //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - shape->calculateLocalInertia(baseMass, baseInertiaDiag); - delete shape; - } - + //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// + for (int i=0;igetNumLinks();i++) + { + btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); + m_multiBody->getLink(i).m_jointFeedback = fb; + m_jointFeedbacks.push_back(fb); + //break; + } - btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); - - m_multiBody = pMultiBody; - btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); - // baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI); - pMultiBody->setBasePos(basePosition); - pMultiBody->setWorldToBaseRot(baseOriQuat); - btVector3 vel(0, 0, 0); - // pMultiBody->setBaseVel(vel); - - //init the links - btVector3 hingeJointAxis(1, 0, 0); - - //y-axis assumed up - btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - ////// - btScalar q0 = 1.f * SIMD_PI/ 180.f; - btQuaternion quat0(btVector3(1, 0, 0).normalized(), q0); - quat0.normalize(); - ///// - - for(int i = 0; i < numLinks; ++i) - { - float linkMass = 1.f; - //if (i==3 || i==2) - // linkMass= 1000; - btVector3 linkInertiaDiag(0.f, 0.f, 0.f); - - btCollisionShape* shape = 0; - if (i==0) - { - shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// - } else - { - shape = new btSphereShape(radius); - } - shape->calculateLocalInertia(linkMass, linkInertiaDiag); - delete shape; - - - if(!spherical) - { - //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); - - if (i==0) - { - pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, - btQuaternion(0.f, 0.f, 0.f, 1.f), - hingeJointAxis, - parentComToCurrentPivot, - currentPivotToCurrentCom, false); - } else - { - btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - - pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, - btQuaternion(0.f, 0.f, 0.f, 1.f), - parentComToCurrentPivot, - currentPivotToCurrentCom); - } - - //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); - - } - else - { - //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); - } - } - - pMultiBody->finalizeMultiDof(); - - //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// - for (int i=0;igetNumLinks();i++) - { - btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); - pMultiBody->getLink(i).m_jointFeedback = fb; - m_jointFeedbacks.push_back(fb); - //break; - } - btMultiBodyDynamicsWorld* world = m_dynamicsWorld; - - /// - world->addMultiBody(pMultiBody); - btMultiBody* mbC = pMultiBody; - mbC->setCanSleep(canSleep); - mbC->setHasSelfCollision(selfCollide); - mbC->setUseGyroTerm(gyro); - // - if(!damping) - { - mbC->setLinearDamping(0.f); - mbC->setAngularDamping(0.f); - }else - { mbC->setLinearDamping(0.1f); - mbC->setAngularDamping(0.9f); - } - // - - - ////////////////////////////////////////////// - if(numLinks > 0) - { - btScalar q0 = 180.f * SIMD_PI/ 180.f; - if(!spherical) - { - mbC->setJointPosMultiDof(0, &q0); - } - else - { - btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); - quat0.normalize(); - mbC->setJointPosMultiDof(0, quat0); - } - } - /// - - btAlignedObjectArray world_to_local; - world_to_local.resize(pMultiBody->getNumLinks() + 1); - - btAlignedObjectArray local_origin; - local_origin.resize(pMultiBody->getNumLinks() + 1); - world_to_local[0] = pMultiBody->getWorldToBaseRot(); - local_origin[0] = pMultiBody->getBasePos(); - double friction = 1; - { - - // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; - float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; - - - if (1) - { - btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]); - m_guiHelper->createCollisionShapeGraphicsObject(shape); - - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); - col->setCollisionShape(shape); - - btTransform tr; - tr.setIdentity(); -//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]); - btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); - - tr.setRotation(orn); - col->setWorldTransform(tr); - - bool isDynamic = (baseMass > 0 && !fixedBase); - 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);//, 2,1+2); - - btVector3 color(0.0,0.0,0.5); - m_guiHelper->createCollisionObjectGraphicsObject(col,color); - -// col->setFriction(friction); - pMultiBody->setBaseCollider(col); - - } - } - - - for (int i=0; i < pMultiBody->getNumLinks(); ++i) - { - const int parent = pMultiBody->getParent(i); - world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; - local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); - } - - - for (int i=0; i < pMultiBody->getNumLinks(); ++i) - { - - btVector3 posr = local_origin[i+1]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; - btCollisionShape* shape =0; - - if (i==0) - { - shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); - } else - { - - shape = new btSphereShape(radius); - } - - m_guiHelper->createCollisionShapeGraphicsObject(shape); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); - - col->setCollisionShape(shape); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - // col->setFriction(friction); - bool isDynamic = 1;//(linkMass > 0); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - //if (i==0||i>numLinks-2) - { - world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2); - btVector4 color = colors[curColor]; - curColor++; - curColor&=3; - m_guiHelper->createCollisionObjectGraphicsObject(col,color); - - - pMultiBody->getLink(i).m_collider=col; - } - - } - } - } char fileName[1024];