add initial inverse dynamics example skeleton, with urdf and programmatically created btMultiBody.

disabled in Bullet/examples/ExampleBrowser/ExampleEntries.cpp

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()

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),

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/*",

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -21,6 +21,7 @@ struct GenericConstraintUserInfo
 
 class MyMultiBodyCreator : public MultiBodyCreationInterface
 {
+protected:
 
 	btMultiBody* m_bulletMultiBody;
     

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));
+}
+
+
+

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

examples/MultiBody/InvertedPendulumPDControl.cpp

@@ -55,38 +55,8 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl()
 extern  bool gJointFeedbackInWorldSpace;
 extern bool gJointFeedbackInJointFrame;
 
-
-
-void InvertedPendulumPDControl::initPhysics()
+btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans)
 {
-	{
-		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);
-    }
-
-	
-
-
-    int upAxis = 1;
-	gJointFeedbackInWorldSpace = true;
-	gJointFeedbackInJointFrame = true;
-
-	m_guiHelper->setUpAxis(upAxis);
-
 	btVector4 colors[4] =
     {
         btVector4(1,0,0,1),
@@ -96,22 +66,6 @@ void InvertedPendulumPDControl::initPhysics()
     };
     int curColor = 0;
 
-
-
-    
-
-	this->createEmptyDynamicsWorld();
-    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
-        //btIDebugDraw::DBG_DrawConstraints
-        +btIDebugDraw::DBG_DrawWireframe
-        +btIDebugDraw::DBG_DrawContactPoints
-        +btIDebugDraw::DBG_DrawAabb
-        );//+btIDebugDraw::DBG_DrawConstraintLimits);
-
-	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
-
-    {
 	bool fixedBase = true;
     bool damping = false;
     bool gyro = false;
@@ -122,7 +76,7 @@ void InvertedPendulumPDControl::initPhysics()
     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);
@@ -139,11 +93,9 @@ void InvertedPendulumPDControl::initPhysics()
         
     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);
+        
+    
+	pMultiBody->setBaseWorldTransform(baseWorldTrans);
     btVector3 vel(0, 0, 0);
 //	pMultiBody->setBaseVel(vel);
 
@@ -216,15 +168,7 @@ void InvertedPendulumPDControl::initPhysics()
 
     pMultiBody->finalizeMultiDof();
 
-		//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
-			for (int i=0;i<pMultiBody->getNumLinks();i++)
-		{
-			btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
-			pMultiBody->getLink(i).m_jointFeedback = fb;
-			m_jointFeedbacks.push_back(fb);
-			//break;
-		}
-        btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
+		
 
     ///
     world->addMultiBody(pMultiBody);
@@ -278,7 +222,7 @@ void InvertedPendulumPDControl::initPhysics()
         if (1)
         {
             btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
-                m_guiHelper->createCollisionShapeGraphicsObject(shape);
+            guiHelper->createCollisionShapeGraphicsObject(shape);
 
             btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
             col->setCollisionShape(shape);
@@ -302,7 +246,7 @@ void InvertedPendulumPDControl::initPhysics()
             world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
 
             btVector3 color(0.0,0.0,0.5);
-                m_guiHelper->createCollisionObjectGraphicsObject(col,color);
+            guiHelper->createCollisionObjectGraphicsObject(col,color);
 
 //                col->setFriction(friction);
             pMultiBody->setBaseCollider(col);
@@ -337,7 +281,7 @@ void InvertedPendulumPDControl::initPhysics()
 			shape = new btSphereShape(radius);
 		}
 
-            m_guiHelper->createCollisionShapeGraphicsObject(shape);
+        guiHelper->createCollisionShapeGraphicsObject(shape);
         btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
 
         col->setCollisionShape(shape);
@@ -357,13 +301,70 @@ void InvertedPendulumPDControl::initPhysics()
 				btVector4 color = colors[curColor];
 		curColor++;
 		curColor&=3;
-            m_guiHelper->createCollisionObjectGraphicsObject(col,color);
+        guiHelper->createCollisionObjectGraphicsObject(col,color);
 
 
         pMultiBody->getLink(i).m_collider=col;
 		}
          
     }
+    
+	return pMultiBody;
+}
+
+void InvertedPendulumPDControl::initPhysics()
+{
+	{
+		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);
+    }
+
+	
+
+
+    int upAxis = 1;
+	gJointFeedbackInWorldSpace = true;
+	gJointFeedbackInJointFrame = true;
+
+	m_guiHelper->setUpAxis(upAxis);
+
+    
+	this->createEmptyDynamicsWorld();
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+        //btIDebugDraw::DBG_DrawConstraints
+        +btIDebugDraw::DBG_DrawWireframe
+        +btIDebugDraw::DBG_DrawContactPoints
+        +btIDebugDraw::DBG_DrawAabb
+        );//+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);
+
+	//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
+	for (int i=0;i<m_multiBody->getNumLinks();i++)
+	{
+		btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
+		m_multiBody->getLink(i).m_jointFeedback = fb;
+		m_jointFeedbacks.push_back(fb);
+		//break;
 	}