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

add real-time safe Inverse Dynamics library+test+utils

Browse Source 
an example for the example browser will follow.
thanks to Thomas Buschmann
This commit is contained in:
Erwin Coumans
2015-11-17 08:27:38 -08:00
parent 7d9365319c
commit aa4d119f98
41 changed files with 5233 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
test/InverseDynamics/premake4.lua Normal file
View File

@@ -0,0 +1,34 @@
project "Test_InverseDynamicsKinematics"
kind "ConsoleApp"
-- defines { }
includedirs
{
".",
"../../src",
"../../examples/InverseDynamics",
"../../Extras/InverseDynamics",
"../gtest-1.7.0/include"
}
if os.is("Windows") then
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
defines {"_VARIADIC_MAX=10"}
end
links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"}
files {
"test_invdyn_kinematics.cpp",
}
if os.is("Linux") then
links {"pthread"}
end

115
test/InverseDynamics/test_invdyn_bullet.cpp Normal file
View File

@@ -0,0 +1,115 @@
/// 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 <random>
#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 <../CommonInterfaces/CommonGUIHelperInterface.h>
#include <gflags/gflags.h>
#include <gtest/gtest.h>
#include <../Importers/ImportURDFDemo/BulletUrdfImporter.h>
#include <../Importers/ImportURDFDemo/URDF2Bullet.h>
#include <../Importers/ImportURDFDemo/MyMultiBodyCreator.h>
#include <../Importers/ImportURDFDemo/URDF2Bullet.h>
#include "../../examples/Utils/b3ResourcePath.h"
#include <invdyn_bullet_comparison.hpp>
#include <btMultiBodyFromURDF.hpp>
#include <MultiBodyTreeCreator.hpp>
#include <MultiBodyTreeDebugGraph.hpp>
using namespace btInverseDynamics;
DEFINE_bool(verbose, false, "print extra info");
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;
std::default_random_engine generator;
std::uniform_real_distribution<double> distribution(-M_PI, M_PI);
auto rnd = std::bind(distribution, generator);
for (int loop = 0; loop < kNLoops; loop++) {
for (int i = 0; i < q.size(); i++) {
q(i) = rnd();
u(i) = rnd();
dot_u(i) = rnd();
}
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) {
gflags::SetUsageMessage("Usage: invdyn_from_btmultibody -verbose = true/false");
gflags::ParseCommandLineFlags(&argc, &argv, false);
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

350
test/InverseDynamics/test_invdyn_kinematics.cpp Normal file
View File

@@ -0,0 +1,350 @@
// Test of kinematic consistency: check if finite differences of velocities, accelerations
// match positions
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <gtest/gtest.h>
#include "../Extras/InverseDynamics/CoilCreator.hpp"
#include "../Extras/InverseDynamics/DillCreator.hpp"
#include "../Extras/InverseDynamics/SimpleTreeCreator.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
using namespace btInverseDynamics;
const int kLevel = 5;
const int kNumBodies = std::pow(2, kLevel);
// template function for calculating the norm
template <typename T>
idScalar calculateNorm(T&);
// only implemented for vec3
template <>
idScalar calculateNorm(vec3& v) {
return std::sqrt(std::pow(v(0), 2) + std::pow(v(1), 2) + std::pow(v(2), 2));
}
// template function to convert a DiffType (finite differences)
// to a ValueType. This is for angular velocity calculations
// via finite differences.
template <typename ValueType, typename DiffType>
DiffType toDiffType(ValueType& fd, ValueType& val);
// vector case: just return finite difference approximation
template <>
vec3 toDiffType(vec3& fd, vec3& val) {
return fd;
}
// orientation case: calculate spin tensor and extract angular velocity
template <>
vec3 toDiffType(mat33& fd, mat33& val) {
// spin tensor
mat33 omega_tilde = fd * val.transpose();
// extract vector from spin tensor
vec3 omega;
omega(0) = 0.5 * (omega_tilde(2, 1) - omega_tilde(1, 2));
omega(1) = 0.5 * (omega_tilde(0, 2) - omega_tilde(2, 0));
omega(2) = 0.5 * (omega_tilde(1, 0) - omega_tilde(0, 1));
return omega;
}
/// Class for calculating finite difference approximation
/// of time derivatives and comparing it to an analytical solution
/// DiffType and ValueType can be different, to allow comparison
/// of angular velocity vectors and orientations given as transform matrices.
template <typename ValueType, typename DiffType>
class DiffFD {
public:
DiffFD() : m_dt(0.0), m_num_updates(0), m_max_error(0.0), m_max_value(0.0), m_valid_fd(false) {}
void init(std::string name, idScalar dt) {
m_name = name;
m_dt = dt;
m_num_updates = 0;
m_max_error = 0.0;
m_max_value = 0.0;
m_valid_fd = false;
}
void update(const ValueType& val, const DiffType& true_diff) {
m_val = val;
if (m_num_updates > 2) {
// 2nd order finite difference approximation for d(value)/dt
ValueType diff_value_fd = (val - m_older_val) / (2.0 * m_dt);
// convert to analytical diff type. This is for angular velocities
m_diff_fd = toDiffType<ValueType, DiffType>(diff_value_fd, m_old_val);
// now, calculate the error
DiffType error_value_type = m_diff_fd - m_old_true_diff;
idScalar error = calculateNorm<DiffType>(error_value_type);
if (error > m_max_error) {
m_max_error = error;
}
idScalar value = calculateNorm<DiffType>(m_old_true_diff);
if (value > m_max_value) {
m_max_value = value;
}
m_valid_fd = true;
}
m_older_val = m_old_val;
m_old_val = m_val;
m_old_true_diff = true_diff;
m_num_updates++;
m_time += m_dt;
}
void printMaxError() {
printf("max_error: %e dt= %e max_value= %e fraction= %e\n", m_max_error, m_dt, m_max_value,
m_max_value > 0.0 ? m_max_error / m_max_value : 0.0);
}
void printCurrent() {
if (m_valid_fd) {
// note: m_old_true_diff already equals m_true_diff here, so values are not aligned.
// (but error calculation takes this into account)
printf("%s time: %e fd: %e %e %e true: %e %e %e\n", m_name.c_str(), m_time,
m_diff_fd(0), m_diff_fd(1), m_diff_fd(2), m_old_true_diff(0), m_old_true_diff(1),
m_old_true_diff(2));
}
}
idScalar getMaxError() const { return m_max_error; }
idScalar getMaxValue() const { return m_max_value; }
private:
idScalar m_dt;
ValueType m_val;
ValueType m_old_val;
ValueType m_older_val;
DiffType m_old_true_diff;
DiffType m_diff_fd;
int m_num_updates;
idScalar m_max_error;
idScalar m_max_value;
idScalar m_time;
std::string m_name;
bool m_valid_fd;
};
template <typename ValueType, typename DiffType>
class VecDiffFD {
public:
VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) {
for (int i = 0; i < m_fd.size(); i++) {
char buf[256];
snprintf(buf, 256, "%s-%.2d", name.c_str(), i);
m_fd[i].init(buf, dt);
}
}
void update(int i, ValueType& val, DiffType& true_diff) { m_fd[i].update(val, true_diff); }
idScalar getMaxError() const {
idScalar max_error = 0;
for (int i = 0; i < m_fd.size(); i++) {
const idScalar error = m_fd[i].getMaxError();
if (error > max_error) {
max_error = error;
}
}
return max_error;
}
void printMaxError() {
printf("%s: total dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError());
}
void printCurrent() {
for (int i = 0; i < m_fd.size(); i++) {
m_fd[i].printCurrent();
}
}
private:
std::string m_name;
std::vector<DiffFD<ValueType, DiffType> > m_fd;
const idScalar m_dt;
idScalar m_max_error;
};
// calculate maximum difference between finite difference and analytical differentiation
int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar deltaT,
idScalar endTime, idScalar* max_linear_velocity_error,
idScalar* max_angular_velocity_error,
idScalar* max_linear_acceleration_error,
idScalar* max_angular_acceleration_error) {
// setup system
MultiBodyTree* tree = CreateMultiBodyTree(creator);
if (0x0 == tree) {
return -1;
}
// set gravity to zero, so nothing is added to accelerations in forward kinematics
vec3 gravity_zero;
gravity_zero(0) = 0;
gravity_zero(1) = 0;
gravity_zero(2) = 0;
tree->setGravityInWorldFrame(gravity_zero);
//
const idScalar kAmplitude = 1.0;
const idScalar kFrequency = 1.0;
vecx q(tree->numDoFs());
vecx dot_q(tree->numDoFs());
vecx ddot_q(tree->numDoFs());
vecx joint_forces(tree->numDoFs());
VecDiffFD<vec3, vec3> fd_vel("linear-velocity", tree->numBodies(), deltaT);
VecDiffFD<vec3, vec3> fd_acc("linear-acceleration", tree->numBodies(), deltaT);
VecDiffFD<mat33, vec3> fd_omg("angular-velocity", tree->numBodies(), deltaT);
VecDiffFD<vec3, vec3> fd_omgd("angular-acceleration", tree->numBodies(), deltaT);
for (idScalar t = 0.0; t < endTime; t += deltaT) {
for (int body = 0; body < tree->numBodies(); body++) {
q(body) = kAmplitude * sin(t * 2.0 * M_PI * kFrequency);
dot_q(body) = kAmplitude * 2.0 * M_PI * kFrequency * cos(t * 2.0 * M_PI * kFrequency);
ddot_q(body) =
-kAmplitude * pow(2.0 * M_PI * kFrequency, 2) * sin(t * 2.0 * M_PI * kFrequency);
}
if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) {
delete tree;
return -1;
}
// position/velocity
for (int body = 0; body < tree->numBodies(); body++) {
vec3 pos;
vec3 vel;
mat33 world_T_body;
vec3 omega;
vec3 dot_omega;
vec3 acc;
tree->getBodyOrigin(body, &pos);
tree->getBodyTransform(body, &world_T_body);
tree->getBodyLinearVelocity(body, &vel);
tree->getBodyAngularVelocity(body, &omega);
tree->getBodyLinearAcceleration(body, &acc);
tree->getBodyAngularAcceleration(body, &dot_omega);
fd_vel.update(body, pos, vel);
fd_omg.update(body, world_T_body, omega);
fd_acc.update(body, vel, acc);
fd_omgd.update(body, omega, dot_omega);
}
}
*max_linear_velocity_error = fd_vel.getMaxError();
*max_angular_velocity_error = fd_omg.getMaxError();
*max_linear_acceleration_error = fd_acc.getMaxError();
*max_angular_acceleration_error = fd_omgd.getMaxError();
delete tree;
return 0;
}
// first test: absolute difference between numerical and numerial
// differentiation should be small
TEST(InvDynKinematicsDifferentiation, errorAbsolute) {
const idScalar kDeltaT = 1e-7;
const idScalar kDuration = 1e-2;
const idScalar kAcceptableError = 1e-4;
CoilCreator coil_creator(kNumBodies);
DillCreator dill_creator(kLevel);
SimpleTreeCreator simple_creator(kNumBodies);
idScalar max_linear_velocity_error;
idScalar max_angular_velocity_error;
idScalar max_linear_acceleration_error;
idScalar max_angular_acceleration_error;
// test serial chain
calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error,
&max_angular_velocity_error, &max_linear_acceleration_error,
&max_angular_acceleration_error);
EXPECT_LT(max_linear_velocity_error, kAcceptableError);
EXPECT_LT(max_angular_velocity_error, kAcceptableError);
EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
// test branched tree
calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error,
&max_angular_velocity_error, &max_linear_acceleration_error,
&max_angular_acceleration_error);
EXPECT_LT(max_linear_velocity_error, kAcceptableError);
EXPECT_LT(max_angular_velocity_error, kAcceptableError);
EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
// test system with different joint types
calculateDifferentiationError(simple_creator, kDeltaT, kDuration, &max_linear_velocity_error,
&max_angular_velocity_error, &max_linear_acceleration_error,
&max_angular_acceleration_error);
EXPECT_LT(max_linear_velocity_error, kAcceptableError);
EXPECT_LT(max_angular_velocity_error, kAcceptableError);
EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
}
// second test: check if the change in the differentiation error
// is consitent with the second order approximation, ie, error ~ O(dt^2)
TEST(InvDynKinematicsDifferentiation, errorOrder) {
const idScalar kDeltaTs[2] = {1e-4, 1e-5};
const idScalar kDuration = 1e-2;
CoilCreator coil_creator(kNumBodies);
// DillCreator dill_creator(kLevel);
// SimpleTreeCreator simple_creator(kNumBodies);
idScalar max_linear_velocity_error[2];
idScalar max_angular_velocity_error[2];
idScalar max_linear_acceleration_error[2];
idScalar max_angular_acceleration_error[2];
// test serial chain
calculateDifferentiationError(coil_creator, kDeltaTs[0], kDuration,
&max_linear_velocity_error[0], &max_angular_velocity_error[0],
&max_linear_acceleration_error[0],
&max_angular_acceleration_error[0]);
calculateDifferentiationError(coil_creator, kDeltaTs[1], kDuration,
&max_linear_velocity_error[1], &max_angular_velocity_error[1],
&max_linear_acceleration_error[1],
&max_angular_acceleration_error[1]);
const idScalar expected_linear_velocity_error_1 =
max_linear_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
const idScalar expected_angular_velocity_error_1 =
max_angular_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
const idScalar expected_linear_acceleration_error_1 =
max_linear_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
const idScalar expected_angular_acceleration_error_1 =
max_angular_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
printf("linear vel error: %e %e %e\n", max_linear_velocity_error[1],
expected_linear_velocity_error_1,
max_linear_velocity_error[1] - expected_linear_velocity_error_1);
printf("angular vel error: %e %e %e\n", max_angular_velocity_error[1],
expected_angular_velocity_error_1,
max_angular_velocity_error[1] - expected_angular_velocity_error_1);
printf("linear acc error: %e %e %e\n", max_linear_acceleration_error[1],
expected_linear_acceleration_error_1,
max_linear_acceleration_error[1] - expected_linear_acceleration_error_1);
printf("angular acc error: %e %e %e\n", max_angular_acceleration_error[1],
expected_angular_acceleration_error_1,
max_angular_acceleration_error[1] - expected_angular_acceleration_error_1);
}
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
return EXIT_SUCCESS;
}