Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
test/InverseDynamics/test_invdyn_bullet.cpp
View File

@@ -32,7 +32,7 @@
using namespace btInverseDynamics;
bool FLAGS_verbose=false;
bool FLAGS_verbose = false;
static btVector3 gravity(0, 0, -10);
static const bool kBaseFixed = false;
@@ -41,73 +41,80 @@ 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);
TEST(InvDynCompare, bulletUrdfR2D2)
{
MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed);
char relativeFileName[1024];
char relativeFileName[1024];
ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024));
ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024));
mb_load.setFileName(relativeFileName);
mb_load.init();
mb_load.setFileName(relativeFileName);
mb_load.init();
btMultiBodyTreeCreator id_creator;
btMultiBody *btmb = mb_load.getBtMultiBody();
ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0);
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);
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());
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;
const int kNLoops = 10;
double max_pos_error = 0;
double max_acc_error = 0;
b3Srand(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);
}
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);
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);
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 (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);
}
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);
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();
int main(int argc, char **argv)
{
b3CommandLineArgs myArgs(argc, argv);
FLAGS_verbose = myArgs.CheckCmdLineFlag("verbose");
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}