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