Code-style consistency improvement:
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
@@ -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();
|
||||
}
|
||||
|
||||
@@ -22,305 +22,340 @@ using namespace btInverseDynamics;
|
||||
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
|
||||
// minimal smart pointer to make this work for c++2003
|
||||
template <typename T>
|
||||
class ptr {
|
||||
ptr();
|
||||
ptr(const ptr&);
|
||||
class ptr
|
||||
{
|
||||
ptr();
|
||||
ptr(const ptr&);
|
||||
|
||||
public:
|
||||
ptr(T* p) : m_p(p) {};
|
||||
~ptr() { delete m_p; }
|
||||
T& operator*() { return *m_p; }
|
||||
T* operator->() { return m_p; }
|
||||
T*get() {return m_p;}
|
||||
const T*get() const {return m_p;}
|
||||
friend bool operator==(const ptr<T>& lhs, const ptr<T>& rhs) { return rhs.m_p == lhs.m_p; }
|
||||
friend bool operator!=(const ptr<T>& lhs, const ptr<T>& rhs) { return !(rhs.m_p == lhs.m_p);
|
||||
}
|
||||
ptr(T* p) : m_p(p){};
|
||||
~ptr() { delete m_p; }
|
||||
T& operator*() { return *m_p; }
|
||||
T* operator->() { return m_p; }
|
||||
T* get() { return m_p; }
|
||||
const T* get() const { return m_p; }
|
||||
friend bool operator==(const ptr<T>& lhs, const ptr<T>& rhs) { return rhs.m_p == lhs.m_p; }
|
||||
friend bool operator!=(const ptr<T>& lhs, const ptr<T>& rhs)
|
||||
{
|
||||
return !(rhs.m_p == lhs.m_p);
|
||||
}
|
||||
|
||||
private:
|
||||
T* m_p;
|
||||
T* m_p;
|
||||
};
|
||||
|
||||
void calculateDotJacUError(const MultiBodyTreeCreator& creator, const int nloops,
|
||||
double* max_error) {
|
||||
// tree1 is used as reference to compute dot(Jacobian)*u from acceleration(dot(u)=0)
|
||||
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
|
||||
ASSERT_TRUE(0x0 != tree1);
|
||||
CloneTreeCreator clone(tree1.get());
|
||||
// tree2 is used to compute dot(Jacobian)*u using the calculateJacobian function
|
||||
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
|
||||
ASSERT_TRUE(0x0 != tree2);
|
||||
double* max_error)
|
||||
{
|
||||
// tree1 is used as reference to compute dot(Jacobian)*u from acceleration(dot(u)=0)
|
||||
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
|
||||
ASSERT_TRUE(0x0 != tree1);
|
||||
CloneTreeCreator clone(tree1.get());
|
||||
// tree2 is used to compute dot(Jacobian)*u using the calculateJacobian function
|
||||
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
|
||||
ASSERT_TRUE(0x0 != tree2);
|
||||
|
||||
const int ndofs = tree1->numDoFs();
|
||||
const int nbodies = tree1->numBodies();
|
||||
if (ndofs <= 0) {
|
||||
*max_error = 0;
|
||||
return;
|
||||
}
|
||||
const int ndofs = tree1->numDoFs();
|
||||
const int nbodies = tree1->numBodies();
|
||||
if (ndofs <= 0)
|
||||
{
|
||||
*max_error = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
vecx q(ndofs);
|
||||
vecx u(ndofs);
|
||||
vecx dot_u(ndofs);
|
||||
vecx zero(ndofs);
|
||||
setZero(zero);
|
||||
vecx q(ndofs);
|
||||
vecx u(ndofs);
|
||||
vecx dot_u(ndofs);
|
||||
vecx zero(ndofs);
|
||||
setZero(zero);
|
||||
|
||||
double max_lin_error = 0;
|
||||
double max_ang_error = 0;
|
||||
double max_lin_error = 0;
|
||||
double max_ang_error = 0;
|
||||
|
||||
for (int loop = 0; loop < nloops; loop++) {
|
||||
for (int i = 0; i < q.size(); i++) {
|
||||
q(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
u(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
}
|
||||
for (int loop = 0; loop < nloops; loop++)
|
||||
{
|
||||
for (int i = 0; i < q.size(); i++)
|
||||
{
|
||||
q(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
u(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
}
|
||||
|
||||
EXPECT_EQ(0, tree1->calculateKinematics(q, u, zero));
|
||||
EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u));
|
||||
EXPECT_EQ(0, tree2->calculateJacobians(q, u));
|
||||
EXPECT_EQ(0, tree1->calculateKinematics(q, u, zero));
|
||||
EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u));
|
||||
EXPECT_EQ(0, tree2->calculateJacobians(q, u));
|
||||
|
||||
for (int idx = 0; idx < nbodies; idx++) {
|
||||
vec3 tmp1, tmp2;
|
||||
vec3 diff;
|
||||
EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1));
|
||||
EXPECT_EQ(0, tree2->getBodyDotJacobianTransU(idx, &tmp2));
|
||||
diff = tmp1 - tmp2;
|
||||
double lin_error = maxAbs(diff);
|
||||
for (int idx = 0; idx < nbodies; idx++)
|
||||
{
|
||||
vec3 tmp1, tmp2;
|
||||
vec3 diff;
|
||||
EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1));
|
||||
EXPECT_EQ(0, tree2->getBodyDotJacobianTransU(idx, &tmp2));
|
||||
diff = tmp1 - tmp2;
|
||||
double lin_error = maxAbs(diff);
|
||||
|
||||
if (lin_error > max_lin_error) {
|
||||
max_lin_error = lin_error;
|
||||
}
|
||||
if (lin_error > max_lin_error)
|
||||
{
|
||||
max_lin_error = lin_error;
|
||||
}
|
||||
|
||||
EXPECT_EQ(0, tree1->getBodyAngularAcceleration(idx, &tmp1));
|
||||
EXPECT_EQ(0, tree2->getBodyDotJacobianRotU(idx, &tmp2));
|
||||
diff = tmp1 - tmp2;
|
||||
double ang_error = maxAbs(diff);
|
||||
if (ang_error > max_ang_error) {
|
||||
max_ang_error = ang_error;
|
||||
}
|
||||
}
|
||||
}
|
||||
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
|
||||
EXPECT_EQ(0, tree1->getBodyAngularAcceleration(idx, &tmp1));
|
||||
EXPECT_EQ(0, tree2->getBodyDotJacobianRotU(idx, &tmp2));
|
||||
diff = tmp1 - tmp2;
|
||||
double ang_error = maxAbs(diff);
|
||||
if (ang_error > max_ang_error)
|
||||
{
|
||||
max_ang_error = ang_error;
|
||||
}
|
||||
}
|
||||
}
|
||||
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
|
||||
}
|
||||
|
||||
void calculateJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
|
||||
double* max_error) {
|
||||
// tree1 is used as reference to compute the Jacobian from velocities with unit u vectors.
|
||||
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
|
||||
ASSERT_TRUE(0x0 != tree1);
|
||||
// tree2 is used to compute the Jacobians using the calculateJacobian function
|
||||
CloneTreeCreator clone(tree1.get());
|
||||
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
|
||||
ASSERT_TRUE(0x0 != tree2);
|
||||
double* max_error)
|
||||
{
|
||||
// tree1 is used as reference to compute the Jacobian from velocities with unit u vectors.
|
||||
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
|
||||
ASSERT_TRUE(0x0 != tree1);
|
||||
// tree2 is used to compute the Jacobians using the calculateJacobian function
|
||||
CloneTreeCreator clone(tree1.get());
|
||||
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
|
||||
ASSERT_TRUE(0x0 != tree2);
|
||||
|
||||
const int ndofs = tree1->numDoFs();
|
||||
const int nbodies = tree1->numBodies();
|
||||
const int ndofs = tree1->numDoFs();
|
||||
const int nbodies = tree1->numBodies();
|
||||
|
||||
if (ndofs <= 0) {
|
||||
*max_error = 0;
|
||||
return;
|
||||
}
|
||||
if (ndofs <= 0)
|
||||
{
|
||||
*max_error = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
vecx q(ndofs);
|
||||
vecx zero(ndofs);
|
||||
setZero(zero);
|
||||
vecx one(ndofs);
|
||||
vecx q(ndofs);
|
||||
vecx zero(ndofs);
|
||||
setZero(zero);
|
||||
vecx one(ndofs);
|
||||
|
||||
double max_lin_error = 0;
|
||||
double max_ang_error = 0;
|
||||
double max_lin_error = 0;
|
||||
double max_ang_error = 0;
|
||||
|
||||
for (int loop = 0; loop < nloops; loop++) {
|
||||
for (int i = 0; i < q.size(); i++) {
|
||||
q(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
}
|
||||
for (int loop = 0; loop < nloops; loop++)
|
||||
{
|
||||
for (int i = 0; i < q.size(); i++)
|
||||
{
|
||||
q(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
}
|
||||
|
||||
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
|
||||
EXPECT_EQ(0, tree2->calculateJacobians(q));
|
||||
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
|
||||
EXPECT_EQ(0, tree2->calculateJacobians(q));
|
||||
|
||||
for (int idx = 0; idx < nbodies; idx++) {
|
||||
mat3x ref_jac_r(3, ndofs);
|
||||
mat3x ref_jac_t(3, ndofs);
|
||||
ref_jac_r.setZero();
|
||||
ref_jac_t.setZero();
|
||||
// this re-computes all jacobians for every body ...
|
||||
// but avoids std::vector<eigen matrix> issues
|
||||
for (int col = 0; col < ndofs; col++) {
|
||||
setZero(one);
|
||||
one(col) = 1.0;
|
||||
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, one));
|
||||
vec3 vel, omg;
|
||||
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel));
|
||||
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg));
|
||||
setMat3xElem(0, col, omg(0), &ref_jac_r);
|
||||
setMat3xElem(1, col, omg(1), &ref_jac_r);
|
||||
setMat3xElem(2, col, omg(2), &ref_jac_r);
|
||||
setMat3xElem(0, col, vel(0), &ref_jac_t);
|
||||
setMat3xElem(1, col, vel(1), &ref_jac_t);
|
||||
setMat3xElem(2, col, vel(2), &ref_jac_t);
|
||||
}
|
||||
for (int idx = 0; idx < nbodies; idx++)
|
||||
{
|
||||
mat3x ref_jac_r(3, ndofs);
|
||||
mat3x ref_jac_t(3, ndofs);
|
||||
ref_jac_r.setZero();
|
||||
ref_jac_t.setZero();
|
||||
// this re-computes all jacobians for every body ...
|
||||
// but avoids std::vector<eigen matrix> issues
|
||||
for (int col = 0; col < ndofs; col++)
|
||||
{
|
||||
setZero(one);
|
||||
one(col) = 1.0;
|
||||
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, one));
|
||||
vec3 vel, omg;
|
||||
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel));
|
||||
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg));
|
||||
setMat3xElem(0, col, omg(0), &ref_jac_r);
|
||||
setMat3xElem(1, col, omg(1), &ref_jac_r);
|
||||
setMat3xElem(2, col, omg(2), &ref_jac_r);
|
||||
setMat3xElem(0, col, vel(0), &ref_jac_t);
|
||||
setMat3xElem(1, col, vel(1), &ref_jac_t);
|
||||
setMat3xElem(2, col, vel(2), &ref_jac_t);
|
||||
}
|
||||
|
||||
mat3x jac_r(3, ndofs);
|
||||
mat3x jac_t(3, ndofs);
|
||||
mat3x diff(3, ndofs);
|
||||
mat3x jac_r(3, ndofs);
|
||||
mat3x jac_t(3, ndofs);
|
||||
mat3x diff(3, ndofs);
|
||||
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t));
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r));
|
||||
sub(ref_jac_t,jac_t,&diff);
|
||||
double lin_error = maxAbsMat3x(diff);
|
||||
if (lin_error > max_lin_error) {
|
||||
max_lin_error = lin_error;
|
||||
}
|
||||
sub(ref_jac_r, jac_r,&diff);
|
||||
double ang_error = maxAbsMat3x(diff);
|
||||
if (ang_error > max_ang_error) {
|
||||
max_ang_error = ang_error;
|
||||
}
|
||||
}
|
||||
}
|
||||
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t));
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r));
|
||||
sub(ref_jac_t, jac_t, &diff);
|
||||
double lin_error = maxAbsMat3x(diff);
|
||||
if (lin_error > max_lin_error)
|
||||
{
|
||||
max_lin_error = lin_error;
|
||||
}
|
||||
sub(ref_jac_r, jac_r, &diff);
|
||||
double ang_error = maxAbsMat3x(diff);
|
||||
if (ang_error > max_ang_error)
|
||||
{
|
||||
max_ang_error = ang_error;
|
||||
}
|
||||
}
|
||||
}
|
||||
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
|
||||
}
|
||||
|
||||
void calculateVelocityJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
|
||||
double* max_error) {
|
||||
// tree1 is used as reference to compute the velocities directly
|
||||
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
|
||||
ASSERT_TRUE(0x0 != tree1);
|
||||
// tree2 is used to compute the velocities via jacobians
|
||||
CloneTreeCreator clone(tree1.get());
|
||||
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
|
||||
ASSERT_TRUE(0x0 != tree2);
|
||||
double* max_error)
|
||||
{
|
||||
// tree1 is used as reference to compute the velocities directly
|
||||
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
|
||||
ASSERT_TRUE(0x0 != tree1);
|
||||
// tree2 is used to compute the velocities via jacobians
|
||||
CloneTreeCreator clone(tree1.get());
|
||||
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
|
||||
ASSERT_TRUE(0x0 != tree2);
|
||||
|
||||
const int ndofs = tree1->numDoFs();
|
||||
const int nbodies = tree1->numBodies();
|
||||
const int ndofs = tree1->numDoFs();
|
||||
const int nbodies = tree1->numBodies();
|
||||
|
||||
if (ndofs <= 0) {
|
||||
*max_error = 0;
|
||||
return;
|
||||
}
|
||||
if (ndofs <= 0)
|
||||
{
|
||||
*max_error = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
vecx q(ndofs);
|
||||
vecx u(ndofs);
|
||||
vecx q(ndofs);
|
||||
vecx u(ndofs);
|
||||
|
||||
double max_lin_error = 0;
|
||||
double max_ang_error = 0;
|
||||
double max_lin_error = 0;
|
||||
double max_ang_error = 0;
|
||||
|
||||
for (int loop = 0; loop < nloops; loop++) {
|
||||
for (int i = 0; i < q.size(); i++) {
|
||||
q(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
u(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
}
|
||||
for (int loop = 0; loop < nloops; loop++)
|
||||
{
|
||||
for (int i = 0; i < q.size(); i++)
|
||||
{
|
||||
q(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
u(i) = b3RandRange(-B3_PI, B3_PI);
|
||||
}
|
||||
|
||||
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, u));
|
||||
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
|
||||
EXPECT_EQ(0, tree2->calculateJacobians(q));
|
||||
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, u));
|
||||
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
|
||||
EXPECT_EQ(0, tree2->calculateJacobians(q));
|
||||
|
||||
for (int idx = 0; idx < nbodies; idx++) {
|
||||
vec3 vel1;
|
||||
vec3 omg1;
|
||||
vec3 vel2;
|
||||
vec3 omg2;
|
||||
mat3x jac_r2(3, ndofs);
|
||||
mat3x jac_t2(3, ndofs);
|
||||
for (int idx = 0; idx < nbodies; idx++)
|
||||
{
|
||||
vec3 vel1;
|
||||
vec3 omg1;
|
||||
vec3 vel2;
|
||||
vec3 omg2;
|
||||
mat3x jac_r2(3, ndofs);
|
||||
mat3x jac_t2(3, ndofs);
|
||||
|
||||
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel1));
|
||||
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg1));
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t2));
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r2));
|
||||
omg2 = jac_r2 * u;
|
||||
vel2 = jac_t2 * u;
|
||||
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel1));
|
||||
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg1));
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t2));
|
||||
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r2));
|
||||
omg2 = jac_r2 * u;
|
||||
vel2 = jac_t2 * u;
|
||||
|
||||
double lin_error = maxAbs(vel1 - vel2);
|
||||
if (lin_error > max_lin_error) {
|
||||
max_lin_error = lin_error;
|
||||
}
|
||||
double ang_error = maxAbs(omg1 - omg2);
|
||||
if (ang_error > max_ang_error) {
|
||||
max_ang_error = ang_error;
|
||||
}
|
||||
}
|
||||
}
|
||||
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
|
||||
double lin_error = maxAbs(vel1 - vel2);
|
||||
if (lin_error > max_lin_error)
|
||||
{
|
||||
max_lin_error = lin_error;
|
||||
}
|
||||
double ang_error = maxAbs(omg1 - omg2);
|
||||
if (ang_error > max_ang_error)
|
||||
{
|
||||
max_ang_error = ang_error;
|
||||
}
|
||||
}
|
||||
}
|
||||
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
|
||||
}
|
||||
|
||||
// test nonlinear terms: dot(Jacobian)*u (linear and angular acceleration for dot_u ==0)
|
||||
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
|
||||
TEST(InvDynJacobians, JacDotJacU) {
|
||||
const int kNumLevels = 5;
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
const double kMaxError = 1e-12;
|
||||
#else
|
||||
const double kMaxError = 5e-5;
|
||||
#endif
|
||||
const int kNumLoops = 20;
|
||||
for (int level = 0; level < kNumLevels; level++) {
|
||||
const int nbodies = BT_ID_POW(2, level);
|
||||
CoilCreator coil(nbodies);
|
||||
double error;
|
||||
calculateDotJacUError(coil, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
DillCreator dill(level);
|
||||
calculateDotJacUError(dill, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
|
||||
const int kRandomLoops = 100;
|
||||
const int kMaxRandomBodies = 128;
|
||||
for (int loop = 0; loop < kRandomLoops; loop++) {
|
||||
RandomTreeCreator random(kMaxRandomBodies);
|
||||
double error;
|
||||
calculateDotJacUError(random, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
}
|
||||
|
||||
// Jacobians: linear and angular acceleration for dot_u ==0
|
||||
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
|
||||
TEST(InvDynJacobians, Jacobians) {
|
||||
const int kNumLevels = 5;
|
||||
TEST(InvDynJacobians, JacDotJacU)
|
||||
{
|
||||
const int kNumLevels = 5;
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
const double kMaxError = 1e-12;
|
||||
#else
|
||||
const double kMaxError = 5e-5;
|
||||
#endif
|
||||
const int kNumLoops = 20;
|
||||
for (int level = 0; level < kNumLevels; level++) {
|
||||
const int nbodies = BT_ID_POW(2, level);
|
||||
CoilCreator coil(nbodies);
|
||||
double error;
|
||||
calculateJacobianError(coil, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
DillCreator dill(level);
|
||||
calculateDotJacUError(dill, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
const int kRandomLoops = 20;
|
||||
const int kMaxRandomBodies = 16;
|
||||
for (int loop = 0; loop < kRandomLoops; loop++) {
|
||||
RandomTreeCreator random(kMaxRandomBodies);
|
||||
double error;
|
||||
calculateJacobianError(random, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
for (int level = 0; level < kNumLevels; level++)
|
||||
{
|
||||
const int nbodies = BT_ID_POW(2, level);
|
||||
CoilCreator coil(nbodies);
|
||||
double error;
|
||||
calculateDotJacUError(coil, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
DillCreator dill(level);
|
||||
calculateDotJacUError(dill, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
|
||||
const int kRandomLoops = 100;
|
||||
const int kMaxRandomBodies = 128;
|
||||
for (int loop = 0; loop < kRandomLoops; loop++)
|
||||
{
|
||||
RandomTreeCreator random(kMaxRandomBodies);
|
||||
double error;
|
||||
calculateDotJacUError(random, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
}
|
||||
|
||||
// test for jacobian*u == velocity
|
||||
TEST(InvDynJacobians, VelocitiesFromJacobians) {
|
||||
const int kRandomLoops = 20;
|
||||
const int kMaxRandomBodies = 16;
|
||||
const int kNumLoops = 20;
|
||||
// Jacobians: linear and angular acceleration for dot_u ==0
|
||||
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
|
||||
TEST(InvDynJacobians, Jacobians)
|
||||
{
|
||||
const int kNumLevels = 5;
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
const double kMaxError = 1e-12;
|
||||
#else
|
||||
const double kMaxError = 5e-5;
|
||||
#endif
|
||||
for (int loop = 0; loop < kRandomLoops; loop++) {
|
||||
RandomTreeCreator random(kMaxRandomBodies);
|
||||
double error;
|
||||
calculateVelocityJacobianError(random, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
const int kNumLoops = 20;
|
||||
for (int level = 0; level < kNumLevels; level++)
|
||||
{
|
||||
const int nbodies = BT_ID_POW(2, level);
|
||||
CoilCreator coil(nbodies);
|
||||
double error;
|
||||
calculateJacobianError(coil, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
DillCreator dill(level);
|
||||
calculateDotJacUError(dill, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
const int kRandomLoops = 20;
|
||||
const int kMaxRandomBodies = 16;
|
||||
for (int loop = 0; loop < kRandomLoops; loop++)
|
||||
{
|
||||
RandomTreeCreator random(kMaxRandomBodies);
|
||||
double error;
|
||||
calculateJacobianError(random, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
}
|
||||
|
||||
// test for jacobian*u == velocity
|
||||
TEST(InvDynJacobians, VelocitiesFromJacobians)
|
||||
{
|
||||
const int kRandomLoops = 20;
|
||||
const int kMaxRandomBodies = 16;
|
||||
const int kNumLoops = 20;
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
const double kMaxError = 1e-12;
|
||||
#else
|
||||
const double kMaxError = 5e-5;
|
||||
#endif
|
||||
for (int loop = 0; loop < kRandomLoops; loop++)
|
||||
{
|
||||
RandomTreeCreator random(kMaxRandomBodies);
|
||||
double error;
|
||||
calculateVelocityJacobianError(random, kNumLoops, &error);
|
||||
EXPECT_GT(kMaxError, error);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
b3Srand(1234);
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
@@ -23,8 +23,9 @@ template <typename T>
|
||||
idScalar calculateNorm(T&);
|
||||
// only implemented for vec3
|
||||
template <>
|
||||
idScalar calculateNorm(vec3& v) {
|
||||
return BT_ID_SQRT(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
|
||||
idScalar calculateNorm(vec3& v)
|
||||
{
|
||||
return BT_ID_SQRT(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
|
||||
}
|
||||
|
||||
// template function to convert a DiffType (finite differences)
|
||||
@@ -35,21 +36,23 @@ DiffType toDiffType(ValueType& fd, ValueType& val);
|
||||
|
||||
// vector case: just return finite difference approximation
|
||||
template <>
|
||||
vec3 toDiffType(vec3& fd, vec3& val) {
|
||||
return fd;
|
||||
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;
|
||||
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
|
||||
@@ -57,290 +60,317 @@ vec3 toDiffType(mat33& fd, mat33& val) {
|
||||
/// 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 {
|
||||
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) {}
|
||||
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 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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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));
|
||||
}
|
||||
}
|
||||
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; }
|
||||
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;
|
||||
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 {
|
||||
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];
|
||||
BT_ID_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;
|
||||
}
|
||||
idScalar getMaxValue() const {
|
||||
idScalar max_value = 0;
|
||||
for (int i = 0; i < m_fd.size(); i++) {
|
||||
const idScalar value = m_fd[i].getMaxValue();
|
||||
if (value > max_value) {
|
||||
max_value= value;
|
||||
}
|
||||
}
|
||||
return max_value;
|
||||
}
|
||||
void printMaxError() {
|
||||
printf("%s: total dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError());
|
||||
}
|
||||
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];
|
||||
BT_ID_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;
|
||||
}
|
||||
idScalar getMaxValue() const
|
||||
{
|
||||
idScalar max_value = 0;
|
||||
for (int i = 0; i < m_fd.size(); i++)
|
||||
{
|
||||
const idScalar value = m_fd[i].getMaxValue();
|
||||
if (value > max_value)
|
||||
{
|
||||
max_value = value;
|
||||
}
|
||||
}
|
||||
return max_value;
|
||||
}
|
||||
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();
|
||||
}
|
||||
}
|
||||
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;
|
||||
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;
|
||||
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());
|
||||
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);
|
||||
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 * BT_ID_PI * kFrequency);
|
||||
dot_q(body) = kAmplitude * 2.0 * BT_ID_PI * kFrequency * cos(t * 2.0 * BT_ID_PI * kFrequency);
|
||||
ddot_q(body) =
|
||||
-kAmplitude * pow(2.0 * BT_ID_PI * kFrequency, 2) * sin(t * 2.0 * BT_ID_PI * kFrequency);
|
||||
}
|
||||
for (idScalar t = 0.0; t < endTime; t += deltaT)
|
||||
{
|
||||
for (int body = 0; body < tree->numBodies(); body++)
|
||||
{
|
||||
q(body) = kAmplitude * sin(t * 2.0 * BT_ID_PI * kFrequency);
|
||||
dot_q(body) = kAmplitude * 2.0 * BT_ID_PI * kFrequency * cos(t * 2.0 * BT_ID_PI * kFrequency);
|
||||
ddot_q(body) =
|
||||
-kAmplitude * pow(2.0 * BT_ID_PI * kFrequency, 2) * sin(t * 2.0 * BT_ID_PI * kFrequency);
|
||||
}
|
||||
|
||||
if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) {
|
||||
delete tree;
|
||||
return -1;
|
||||
}
|
||||
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;
|
||||
// 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);
|
||||
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);
|
||||
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);
|
||||
|
||||
// fd_vel.printCurrent();
|
||||
//fd_acc.printCurrent();
|
||||
//fd_omg.printCurrent();
|
||||
//fd_omgd.printCurrent();
|
||||
}
|
||||
}
|
||||
|
||||
// fd_vel.printCurrent();
|
||||
//fd_acc.printCurrent();
|
||||
//fd_omg.printCurrent();
|
||||
//fd_omgd.printCurrent();
|
||||
}
|
||||
}
|
||||
*max_linear_velocity_error = fd_vel.getMaxError() / fd_vel.getMaxValue();
|
||||
*max_angular_velocity_error = fd_omg.getMaxError() / fd_omg.getMaxValue();
|
||||
*max_linear_acceleration_error = fd_acc.getMaxError() / fd_acc.getMaxValue();
|
||||
*max_angular_acceleration_error = fd_omgd.getMaxError() / fd_omgd.getMaxValue();
|
||||
|
||||
*max_linear_velocity_error = fd_vel.getMaxError()/fd_vel.getMaxValue();
|
||||
*max_angular_velocity_error = fd_omg.getMaxError()/fd_omg.getMaxValue();
|
||||
*max_linear_acceleration_error = fd_acc.getMaxError()/fd_acc.getMaxValue();
|
||||
*max_angular_acceleration_error = fd_omgd.getMaxError()/fd_omgd.getMaxValue();
|
||||
|
||||
delete tree;
|
||||
return 0;
|
||||
delete tree;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// first test: absolute difference between numerical and numerial
|
||||
// differentiation should be small
|
||||
TEST(InvDynKinematicsDifferentiation, errorAbsolute) {
|
||||
//CAVEAT:these values are hand-tuned to work for the specific trajectory defined above.
|
||||
TEST(InvDynKinematicsDifferentiation, errorAbsolute)
|
||||
{
|
||||
//CAVEAT:these values are hand-tuned to work for the specific trajectory defined above.
|
||||
#ifdef BT_ID_USE_DOUBLE_PRECISION
|
||||
const idScalar kDeltaT = 1e-7;
|
||||
const idScalar kDeltaT = 1e-7;
|
||||
const idScalar kAcceptableError = 1e-4;
|
||||
#else
|
||||
const idScalar kDeltaT = 1e-4;
|
||||
const idScalar kDeltaT = 1e-4;
|
||||
const idScalar kAcceptableError = 5e-3;
|
||||
#endif
|
||||
const idScalar kDuration = 0.01;
|
||||
|
||||
const idScalar kDuration = 0.01;
|
||||
|
||||
CoilCreator coil_creator(kNumBodies);
|
||||
DillCreator dill_creator(kLevel);
|
||||
SimpleTreeCreator simple_creator(kNumBodies);
|
||||
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;
|
||||
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);
|
||||
// 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);
|
||||
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(dill_creator, kDeltaT, kDuration, &max_linear_velocity_error,
|
||||
&max_angular_velocity_error, &max_linear_acceleration_error,
|
||||
&max_angular_acceleration_error);
|
||||
// test branched tree
|
||||
calculateDifferentiationError(dill_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);
|
||||
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);
|
||||
// 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);
|
||||
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;
|
||||
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);
|
||||
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];
|
||||
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]);
|
||||
// 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]);
|
||||
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 =
|
||||
@@ -365,10 +395,10 @@ TEST(InvDynKinematicsDifferentiation, errorOrder) {
|
||||
*/
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user