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
102
103
104
105
106
107
108
109
110
111
112
113
|
/// create a bullet btMultiBody model of a tree structured multibody system,
/// convert that model to a MultiBodyTree model.
/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces
/// - run forward dynamics on (q,u, forces) to get accelerations
/// - compare input accelerations to inverse dynamics to output from forward dynamics
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <string>
#include <btBulletDynamicsCommon.h>
#include <btMultiBodyTreeCreator.hpp>
#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
#include <gtest/gtest.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/b3ResourcePath.h"
#include <invdyn_bullet_comparison.hpp>
#include <btMultiBodyFromURDF.hpp>
#include <MultiBodyTreeCreator.hpp>
#include <MultiBodyTreeDebugGraph.hpp>
#include "Bullet3Common/b3CommandLineArgs.h"
#include "Bullet3Common/b3Random.h"
using namespace btInverseDynamics;
bool FLAGS_verbose=false;
static btVector3 gravity(0, 0, -10);
static const bool kBaseFixed = false;
static const char kUrdfFile[] = "r2d2.urdf";
/// this test loads the a urdf model with fixed, floating, prismatic and rotational joints,
/// converts in to an inverse dynamics model and compares forward to inverse dynamics for
/// random input
TEST(InvDynCompare, bulletUrdfR2D2) {
MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed);
char relativeFileName[1024];
ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024));
mb_load.setFileName(relativeFileName);
mb_load.init();
btMultiBodyTreeCreator id_creator;
btMultiBody *btmb = mb_load.getBtMultiBody();
ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0);
MultiBodyTree *id_tree = CreateMultiBodyTree(id_creator);
ASSERT_EQ(0x0 != id_tree, true);
vecx q(id_tree->numDoFs());
vecx u(id_tree->numDoFs());
vecx dot_u(id_tree->numDoFs());
vecx joint_forces(id_tree->numDoFs());
const int kNLoops = 10;
double max_pos_error = 0;
double max_acc_error = 0;
b3Srand(0);
for (int loop = 0; loop < kNLoops; loop++) {
for (int i = 0; i < q.size(); i++) {
q(i) = b3RandRange(-B3_PI, B3_PI);
u(i) = b3RandRange(-B3_PI, B3_PI);
dot_u(i) = b3RandRange(-B3_PI, B3_PI);
}
double pos_error;
double acc_error;
btmb->clearForcesAndTorques();
id_tree->clearAllUserForcesAndMoments();
// call inverse dynamics once, to get global position & velocity of root body
// (fixed, so q, u, dot_u arbitrary)
EXPECT_EQ(id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces), 0);
EXPECT_EQ(compareInverseAndForwardDynamics(q, u, dot_u, gravity, FLAGS_verbose, btmb, id_tree,
&pos_error, &acc_error),
0);
if (pos_error > max_pos_error) {
max_pos_error = pos_error;
}
if (acc_error > max_acc_error) {
max_acc_error = acc_error;
}
}
if (FLAGS_verbose) {
printf("max_pos_error= %e\n", max_pos_error);
printf("max_acc_error= %e\n", max_acc_error);
}
EXPECT_LT(max_pos_error, std::numeric_limits<idScalar>::epsilon()*1e4);
EXPECT_LT(max_acc_error, std::numeric_limits<idScalar>::epsilon()*1e5);
}
int main(int argc, char **argv) {
b3CommandLineArgs myArgs(argc,argv);
FLAGS_verbose = myArgs.CheckCmdLineFlag("verbose");
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
|
