1 #ifndef BTMULTIBODYFROMURDF_HPP 2 #define BTMULTIBODYFROMURDF_HPP 3 4 #include "btBulletDynamicsCommon.h" 5 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" 6 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" 7 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" 8 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" 9 #include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h" 10 #include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h" 11 #include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h" 12 #include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h" 13 #include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h" 14 #include "../../examples/Utils/b3BulletDefaultFileIO.h" 15 /// Create a btMultiBody model from URDF. 16 /// This is adapted from Bullet URDF loader example 17 class MyBtMultiBodyFromURDF 18 { 19 public: 20 /// ctor 21 /// @param gravity gravitational acceleration (in world frame) 22 /// @param base_fixed if true, the root body is treated as fixed, 23 /// if false, it is treated as floating MyBtMultiBodyFromURDF(const btVector3 & gravity,const bool base_fixed)24 MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed) 25 : m_gravity(gravity), m_base_fixed(base_fixed) 26 { 27 m_broadphase = 0x0; 28 m_dispatcher = 0x0; 29 m_solver = 0x0; 30 m_collisionConfiguration = 0x0; 31 m_dynamicsWorld = 0x0; 32 m_multibody = 0x0; 33 m_flag = 0x0; 34 } 35 /// dtor ~MyBtMultiBodyFromURDF()36 ~MyBtMultiBodyFromURDF() 37 { 38 delete m_dynamicsWorld; 39 delete m_solver; 40 delete m_broadphase; 41 delete m_dispatcher; 42 delete m_collisionConfiguration; 43 delete m_multibody; 44 } 45 /// @param name path to urdf file setFileName(const std::string name)46 void setFileName(const std::string name) { m_filename = name; } setFlag(int flag)47 void setFlag(int flag) { m_flag = flag; } 48 /// load urdf file and build btMultiBody model init()49 void init() 50 { 51 this->createEmptyDynamicsWorld(); 52 m_dynamicsWorld->setGravity(m_gravity); 53 b3BulletDefaultFileIO fileIO; 54 BulletURDFImporter urdf_importer(&m_nogfx, 0, &fileIO, 1, 0); 55 URDFImporterInterface &u2b(urdf_importer); 56 bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); 57 58 if (loadOk) 59 { 60 btTransform identityTrans; 61 identityTrans.setIdentity(); 62 MyMultiBodyCreator creation(&m_nogfx); 63 const bool use_multibody = true; 64 ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody, 65 u2b.getPathPrefix(), m_flag); 66 m_multibody = creation.getBulletMultiBody(); 67 m_dynamicsWorld->stepSimulation(1. / 240., 0); 68 } 69 } 70 /// @return pointer to the btMultiBody model getBtMultiBody()71 btMultiBody *getBtMultiBody() { return m_multibody; } 72 73 private: 74 // internal utility function createEmptyDynamicsWorld()75 void createEmptyDynamicsWorld() 76 { 77 m_collisionConfiguration = new btDefaultCollisionConfiguration(); 78 79 /// use the default collision dispatcher. For parallel processing you can use a diffent 80 /// dispatcher (see Extras/BulletMultiThreaded) 81 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); 82 m_broadphase = new btDbvtBroadphase(); 83 m_solver = new btMultiBodyConstraintSolver; 84 m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, 85 m_collisionConfiguration); 86 m_dynamicsWorld->setGravity(m_gravity); 87 } 88 89 btBroadphaseInterface *m_broadphase; 90 btCollisionDispatcher *m_dispatcher; 91 btMultiBodyConstraintSolver *m_solver; 92 btDefaultCollisionConfiguration *m_collisionConfiguration; 93 btMultiBodyDynamicsWorld *m_dynamicsWorld; 94 std::string m_filename; 95 DummyGUIHelper m_nogfx; 96 btMultiBody *m_multibody; 97 const btVector3 m_gravity; 98 const bool m_base_fixed; 99 int m_flag; 100 }; 101 #endif // BTMULTIBODYFROMURDF_HPP 102