From 51938d642e297fb41c0007ebafc1073ef05dea53 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 22 Apr 2015 18:09:00 -0700 Subject: [PATCH] prepare for 'custom' multi body creation. --- examples/ExampleBrowser/ExampleEntries.cpp | 2 + .../ImportURDFDemo/ImportURDFSetup.cpp | 58 +---------------- .../ImportURDFDemo/MyMultiBodyCreator.cpp | 4 +- .../ImportURDFDemo/MyMultiBodyCreator.h | 2 +- .../CustomMultiBodyCreationCallback.h | 42 ++++++++++++ .../MultiBody/MultiBodyCustomURDFDemo.cpp | 64 +++++++++++++++++++ examples/MultiBody/MultiBodyCustomURDFDemo.h | 6 ++ 7 files changed, 118 insertions(+), 60 deletions(-) create mode 100644 examples/MultiBody/CustomMultiBodyCreationCallback.h create mode 100644 examples/MultiBody/MultiBodyCustomURDFDemo.cpp create mode 100644 examples/MultiBody/MultiBodyCustomURDFDemo.h diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a81474f25..58c662cc2 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/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 diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 0cc64e3d2..c91835233 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -172,61 +172,6 @@ void printTree1(my_shared_ptr 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 m_link2rigidbody; - btAlignedObjectArray m_linkMasses; - - bool m_createMultiBody; - int m_totalNumJoints; - btMultiBody* m_bulletMultiBody; - - btAlignedObjectArray 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; diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index fb2ca9dcb..aa1c56503 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/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); diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index 1a5df1b9c..0fc1df2dc 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/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 diff --git a/examples/MultiBody/CustomMultiBodyCreationCallback.h b/examples/MultiBody/CustomMultiBodyCreationCallback.h new file mode 100644 index 000000000..a57da650d --- /dev/null +++ b/examples/MultiBody/CustomMultiBodyCreationCallback.h @@ -0,0 +1,42 @@ +#ifndef CUSTOM_MULTIBODY_CALLBACK_H +#define CUSTOM_MULTIBODY_CALLBACK_H + +#ifdef USE_EIGEN + typedef Eigen::Quaternion QuaternionType; + typedef Eigen::Vector3d 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 diff --git a/examples/MultiBody/MultiBodyCustomURDFDemo.cpp b/examples/MultiBody/MultiBodyCustomURDFDemo.cpp new file mode 100644 index 000000000..1ee37a7db --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/examples/MultiBody/MultiBodyCustomURDFDemo.h b/examples/MultiBody/MultiBodyCustomURDFDemo.h new file mode 100644 index 000000000..e13a0102f --- /dev/null +++ b/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