prepare for 'custom' multi body creation.

examples/ExampleBrowser/ExampleEntries.cpp

@@ -21,6 +21,7 @@
 #include "../Constraints/ConstraintPhysicsSetup.h"
 #include "../MultiBody/TestJointTorqueSetup.h"
 #include "../MultiBody/MultiDofDemo.h"
+#include "../MultiBody/MultiBodyCustomURDFDemo.h"
 
 struct ExampleEntry
 {
@@ -52,6 +53,7 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(0,"MultiBody",0),
 	ExampleEntry(1,"TestJointTorque",TestJointTorqueCreateFunc),
 	ExampleEntry(1,"MultiDofCreateFunc",MultiDofCreateFunc),
+	ExampleEntry(1,"Custom URDF",MultiBodyCustomURDFDofCreateFunc),
 	
 
 #ifndef _DEBUG

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -172,61 +172,6 @@ void printTree1(my_shared_ptr<const Link> link,int level = 0)
 }
 
 
-struct URDF_LinkInformation
-{
-    const Link* m_thisLink;
-	int m_linkIndex;
-	//int m_parentIndex;
-
-    btTransform m_localInertialFrame;
-    //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");
-	}
-};
-
-struct URDF_JointInformation
-{
-
-};
-
-
-struct URDF2BulletMappings
-{
-    btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
-	btAlignedObjectArray<btScalar>			m_linkMasses;
-	
-	bool m_createMultiBody;
-	int m_totalNumJoints;
-	btMultiBody*	m_bulletMultiBody;
-
-    btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
-	URDF2BulletMappings()
-		:m_createMultiBody(false),
-		m_totalNumJoints(0),
-		m_bulletMultiBody(0)
-	{
-	}
-
-};
-
 
 
 void ImportUrdfSetup::initPhysics()
@@ -257,7 +202,6 @@ void ImportUrdfSetup::initPhysics()
 	bool fileFound = fu.findFile(m_fileName, relativeFileName, 1024);
 
 
-
 	std::string xml_string;
 	char pathPrefix[1024];
 	pathPrefix[0] = 0;
@@ -316,7 +260,7 @@ void ImportUrdfSetup::initPhysics()
     bool useUrdfInterfaceClass = true;
     
     {
-        URDF2BulletMappings mappings;
+        
       
         btMultiBody* mb = 0;
         

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -40,8 +40,8 @@ class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider
     btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
     return mbCol;
 }
-    
-    
+
+
 class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) 
 {
     btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -5,7 +5,7 @@
 #include "MultiBodyCreationInterface.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-class GUIHelperInterface;
+struct GUIHelperInterface;
 class btMultiBody;
 
 class MyMultiBodyCreator : public MultiBodyCreationInterface

examples/MultiBody/CustomMultiBodyCreationCallback.h

@@ -0,0 +1,42 @@
+#ifndef CUSTOM_MULTIBODY_CALLBACK_H
+#define CUSTOM_MULTIBODY_CALLBACK_H
+
+#ifdef USE_EIGEN
+	typedef Eigen::Quaternion<double> QuaternionType;
+	typedef Eigen::Vector3d<double> Vector3dType;
+	typedef double ScalarType;
+#else
+	typedef btQuaternion QuaternionType;
+	typedef btVector3 Vector3dType;
+	typedef btScalar	ScalarType;
+#endif
+
+class CustomMultiBodyCreationCallback
+{
+    
+public:
+
+	virtual ~CustomMultiBodyCreationCallback() {}
+
+    enum {
+        RevoluteJoint=1,
+        PrismaticJoint,
+        FixedJoint,
+    };
+ 
+    virtual int allocateMultiBodyBase(int urdfLinkIndex, int totalNumJoints,ScalarType baseMass, const Vector3dType& localInertiaDiagonal, bool isFixedBase) const =0;
+
+    virtual void addLinkAndJoint(int jointType, int linkIndex,            // 0 to num_links-1
+                       int parentIndex,
+                       double mass,
+                       const Vector3dType&	inertia,
+                       const QuaternionType &rotParentFrameToLinkFrame,  // rotate points in parent frame to this frame, when q = 0
+                       const Vector3dType&	jointAxisInLinkFrame,    // in Link frame
+                       const Vector3dType&	parentComToThisJointOffset,    // vector from parent COM to joint frame, in Parent frame
+                       const Vector3dType&	thisJointToThisComOffset) = 0;       // vector from joint frame to my COM, in Link frame);
+                 
+// @todo: Decide if we need this link mapping? 
+// virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const = 0;
+};
+
+#endif //CUSTOM_MULTIBODY_CALLBACK_H

examples/MultiBody/MultiBodyCustomURDFDemo.cpp

@@ -0,0 +1,64 @@
+
+#include "MultiBodyCustomURDFDemo.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "CustomMultiBodyCreationCallback.h"
+
+
+struct MultiBodyCustomURDFDemo : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+
+public:
+
+    MultiBodyCustomURDFDemo(struct GUIHelperInterface* helper);
+    virtual ~MultiBodyCustomURDFDemo();
+
+    virtual void initPhysics();
+
+    virtual void stepSimulation(float deltaTime);
+
+};
+
+MultiBodyCustomURDFDemo::MultiBodyCustomURDFDemo(struct GUIHelperInterface* helper)
+:CommonMultiBodyBase(helper)
+{
+}
+
+MultiBodyCustomURDFDemo::~MultiBodyCustomURDFDemo()
+{
+
+}
+
+void MultiBodyCustomURDFDemo::initPhysics()
+{
+    int upAxis = 2;
+    m_guiHelper->setUpAxis(upAxis);
+	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);
+
+
+    
+}
+
+void MultiBodyCustomURDFDemo::stepSimulation(float deltaTime)
+{
+   
+    m_dynamicsWorld->stepSimulation(deltaTime);
+}
+
+
+class ExampleInterface*    MultiBodyCustomURDFDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option)
+{
+	return new MultiBodyCustomURDFDemo(helper);
+}

examples/MultiBody/MultiBodyCustomURDFDemo.h

@@ -0,0 +1,6 @@
+#ifndef MULTI_DOF_CUSTOM_URDF_DEMO_H
+#define MULTI_DOF_CUSTOM_URDF_DEMO_H
+
+class ExampleInterface*    MultiBodyCustomURDFDofCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
+
+#endif //MULTI_DOF_CUSTOM_URDF_DEMO_H