From ba8964c4acf8a933ec11a049296769157ef390ee Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 25 Aug 2016 16:24:28 -0700 Subject: [PATCH] [InverseDynamics] Support for Jacobians & derivatives This change adds support for calculating Jacobians and dot(Jacobian)*u terms, along with the required support for the 3xN matrices in the standalone math library. It also adds functions to compute kinematics only (position, velocity, accel). To facilitate tests, the Cl also adds a RandomTreeCreator to create randomized multibody trees. Thanks to Thomas Buschmann for this contribution! --- Extras/InverseDynamics/CMakeLists.txt | 2 + Extras/InverseDynamics/CloneTreeCreator.cpp | 49 ++ Extras/InverseDynamics/CloneTreeCreator.hpp | 27 + Extras/InverseDynamics/IDRandomUtil.cpp | 1 + Extras/InverseDynamics/IDRandomUtil.hpp | 4 +- Extras/InverseDynamics/RandomTreeCreator.cpp | 81 +++ Extras/InverseDynamics/RandomTreeCreator.hpp | 31 ++ .../InverseDynamics/btMultiBodyFromURDF.hpp | 14 +- .../btMultiBodyTreeCreator.hpp | 8 +- .../invdyn_bullet_comparison.hpp | 3 +- src/BulletInverseDynamics/IDConfig.hpp | 11 +- src/BulletInverseDynamics/IDConfigEigen.hpp | 15 +- src/BulletInverseDynamics/IDMath.cpp | 54 ++ src/BulletInverseDynamics/IDMath.hpp | 10 + src/BulletInverseDynamics/MultiBodyTree.cpp | 114 +++++ src/BulletInverseDynamics/MultiBodyTree.hpp | 61 ++- .../details/IDEigenInterface.hpp | 37 +- .../details/IDLinearMathInterface.hpp | 69 ++- .../details/IDMatVec.hpp | 93 +++- .../details/MultiBodyTreeImpl.cpp | 465 ++++++++++++------ .../details/MultiBodyTreeImpl.hpp | 47 ++ test/InverseDynamics/CMakeLists.txt | 108 +++- test/InverseDynamics/premake4.lua | 35 +- test/InverseDynamics/test_invdyn_bullet.cpp | 10 +- test/InverseDynamics/test_invdyn_jacobian.cpp | 326 ++++++++++++ 25 files changed, 1474 insertions(+), 201 deletions(-) create mode 100644 Extras/InverseDynamics/CloneTreeCreator.cpp create mode 100644 Extras/InverseDynamics/CloneTreeCreator.hpp create mode 100644 Extras/InverseDynamics/RandomTreeCreator.cpp create mode 100644 Extras/InverseDynamics/RandomTreeCreator.hpp create mode 100644 test/InverseDynamics/test_invdyn_jacobian.cpp diff --git a/Extras/InverseDynamics/CMakeLists.txt b/Extras/InverseDynamics/CMakeLists.txt index 56ed7d76b..1e2031fcb 100644 --- a/Extras/InverseDynamics/CMakeLists.txt +++ b/Extras/InverseDynamics/CMakeLists.txt @@ -4,6 +4,7 @@ INCLUDE_DIRECTORIES( ADD_LIBRARY( BulletInverseDynamicsUtils +CloneTreeCreator.cpp CoilCreator.cpp MultiBodyTreeCreator.cpp btMultiBodyTreeCreator.cpp @@ -11,6 +12,7 @@ DillCreator.cpp MultiBodyTreeDebugGraph.cpp invdyn_bullet_comparison.cpp IDRandomUtil.cpp +RandomTreeCreator.cpp SimpleTreeCreator.cpp MultiBodyNameMap.cpp User2InternalIndex.cpp diff --git a/Extras/InverseDynamics/CloneTreeCreator.cpp b/Extras/InverseDynamics/CloneTreeCreator.cpp new file mode 100644 index 000000000..899b5d996 --- /dev/null +++ b/Extras/InverseDynamics/CloneTreeCreator.cpp @@ -0,0 +1,49 @@ +#include "CloneTreeCreator.hpp" + +#include + +namespace btInverseDynamics { +#define CHECK_NULLPTR() \ + do { \ + if (m_reference == 0x0) { \ + error_message("m_reference == 0x0\n"); \ + return -1; \ + } \ + } while (0) + +#define TRY(x) \ + do { \ + if (x == -1) { \ + error_message("error calling " #x "\n"); \ + return -1; \ + } \ + } while (0) +CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference) { m_reference = reference; } + +CloneTreeCreator::~CloneTreeCreator() {} + +int CloneTreeCreator::getNumBodies(int* num_bodies) const { + CHECK_NULLPTR(); + *num_bodies = m_reference->numBodies(); + return 0; +} + +int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, + vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, + mat33* body_I_body, int* user_int, void** user_ptr) const { + CHECK_NULLPTR(); + TRY(m_reference->getParentIndex(body_index, parent_index)); + TRY(m_reference->getJointType(body_index, joint_type)); + TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref)); + TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref)); + TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion)); + TRY(m_reference->getBodyMass(body_index, mass)); + TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com)); + TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body)); + TRY(m_reference->getUserInt(body_index, user_int)); + TRY(m_reference->getUserPtr(body_index, user_ptr)); + + return 0; +} +} diff --git a/Extras/InverseDynamics/CloneTreeCreator.hpp b/Extras/InverseDynamics/CloneTreeCreator.hpp new file mode 100644 index 000000000..cb96d56b4 --- /dev/null +++ b/Extras/InverseDynamics/CloneTreeCreator.hpp @@ -0,0 +1,27 @@ +#ifndef CLONETREE_CREATOR_HPP_ +#define CLONETREE_CREATOR_HPP_ + +#include "BulletInverseDynamics/IDConfig.hpp" +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { +/// Generate an identical multibody tree from a reference system. +class CloneTreeCreator : public MultiBodyTreeCreator { +public: + /// ctor + /// @param reference the MultiBodyTree to clone + CloneTreeCreator(const MultiBodyTree*reference); + ~CloneTreeCreator(); + ///\copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int* num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, + idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, + void** user_ptr) const; + +private: + const MultiBodyTree *m_reference; +}; +} +#endif // CLONETREE_CREATOR_HPP_ diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp index c3110e5c7..fcf879573 100644 --- a/Extras/InverseDynamics/IDRandomUtil.cpp +++ b/Extras/InverseDynamics/IDRandomUtil.cpp @@ -15,6 +15,7 @@ static const float mass_min = 0.001; static const float mass_max = 1.0; void randomInit() { srand(time(NULL)); } +void randomInit(unsigned seed) { srand(seed); } int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; } diff --git a/Extras/InverseDynamics/IDRandomUtil.hpp b/Extras/InverseDynamics/IDRandomUtil.hpp index a9d363a5b..72a97fa2b 100644 --- a/Extras/InverseDynamics/IDRandomUtil.hpp +++ b/Extras/InverseDynamics/IDRandomUtil.hpp @@ -2,8 +2,10 @@ #define ID_RANDOM_UTIL_HPP_ #include "BulletInverseDynamics/IDConfig.hpp" namespace btInverseDynamics { -/// seed random number generator +/// seed random number generator using time() void randomInit(); +/// seed random number generator with identical value to get repeatable results +void randomInit(unsigned seed); /// Generate (not quite) uniformly distributed random integers in [low, high] /// Note: this is a low-quality implementation using only rand(), as /// C++11 is not supported in bullet. diff --git a/Extras/InverseDynamics/RandomTreeCreator.cpp b/Extras/InverseDynamics/RandomTreeCreator.cpp new file mode 100644 index 000000000..b2731c590 --- /dev/null +++ b/Extras/InverseDynamics/RandomTreeCreator.cpp @@ -0,0 +1,81 @@ +#include "RandomTreeCreator.hpp" + +#include + +#include "IDRandomUtil.hpp" + +namespace btInverseDynamics { + +RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed) { + // seed generator + if(random_seed) { + randomInit(); // seeds with time() + } else { + randomInit(1); // seeds with 1 + } + m_num_bodies = randomInt(1, max_bodies); +} + +RandomTreeCreator::~RandomTreeCreator() {} + +int RandomTreeCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int RandomTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, + vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, + mat33* body_I_body, int* user_int, void** user_ptr) const { + if(0 == body_index) { //root body + *parent_index = -1; + } else { + *parent_index = randomInt(0, body_index - 1); + } + + switch (randomInt(0, 3)) { + case 0: + *joint_type = FIXED; + break; + case 1: + *joint_type = REVOLUTE; + break; + case 2: + *joint_type = PRISMATIC; + break; + case 3: + *joint_type = FLOATING; + break; + default: + error_message("randomInt() result out of range\n"); + return -1; + } + + (*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0); + (*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0); + (*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0); + + bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref); + + *body_axis_of_motion = randomAxis(); + *mass = randomMass(); + (*body_r_body_com)(0) = randomFloat(-1.0, 1.0); + (*body_r_body_com)(1) = randomFloat(-1.0, 1.0); + (*body_r_body_com)(2) = randomFloat(-1.0, 1.0); + const double a = randomFloat(-BT_ID_PI, BT_ID_PI); + const double b = randomFloat(-BT_ID_PI, BT_ID_PI); + const double c = randomFloat(-BT_ID_PI, BT_ID_PI); + vec3 ii = randomInertiaPrincipal(); + mat33 ii_diag; + setZero(ii_diag); + ii_diag(0,0)=ii(0); + ii_diag(1,1)=ii(1); + ii_diag(2,2)=ii(2); + *body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag * + transformZ(-c) * transformY(-b) * transformX(-a); + *user_int = 0; + *user_ptr = 0; + + return 0; +} +} diff --git a/Extras/InverseDynamics/RandomTreeCreator.hpp b/Extras/InverseDynamics/RandomTreeCreator.hpp new file mode 100644 index 000000000..bbadb997d --- /dev/null +++ b/Extras/InverseDynamics/RandomTreeCreator.hpp @@ -0,0 +1,31 @@ +#ifndef RANDOMTREE_CREATOR_HPP_ +#define RANDOMTREE_CREATOR_HPP_ + +#include "BulletInverseDynamics/IDConfig.hpp" +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { +/// Generate a random MultiBodyTree with fixed or floating base and fixed, prismatic or revolute +/// joints +/// Uses a pseudo random number generator seeded from a random device. +class RandomTreeCreator : public MultiBodyTreeCreator { +public: + /// ctor + /// @param max_bodies maximum number of bodies + /// @param gravity gravitational acceleration + /// @param use_seed if true, seed random number generator + RandomTreeCreator(const int max_bodies, bool use_seed=false); + ~RandomTreeCreator(); + ///\copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int* num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, + idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, + void** user_ptr) const; + +private: + int m_num_bodies; +}; +} +#endif // RANDOMTREE_CREATOR_HPP_ diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp index a38ad268b..d627ea985 100644 --- a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -6,12 +6,11 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" -#include "../CommonInterfaces/CommonGUIHelperInterface.h" -#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" -#include "../Importers/ImportURDFDemo/URDF2Bullet.h" -#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" -#include "../Importers/ImportURDFDemo/URDF2Bullet.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" /// Create a btMultiBody model from URDF. /// This is adapted from Bullet URDF loader example @@ -45,8 +44,7 @@ public: void init() { this->createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(m_gravity); - - BulletURDFImporter urdf_importer(&m_nogfx, 0); + BulletURDFImporter urdf_importer(&m_nogfx,0); URDFImporterInterface &u2b(urdf_importer); bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp index 3844cf3d3..dbc82ab3f 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -25,10 +25,10 @@ public: /// \copydoc MultiBodyTreeCreator::getNumBodies int getNumBodies(int *num_bodies) const; ///\copydoc MultiBodyTreeCreator::getBody - int getBody(const int body_index, int *parent_index, JointType *joint_type, - vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion, - idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int, - void **user_ptr) const; + int getBody(const int body_index, int *parent_index, JointType *joint_type, + vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, + vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com, + mat33 *body_I_body, int *user_int, void **user_ptr) const; private: // internal struct holding data extracted from btMultiBody diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.hpp b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp index 2a74af59e..1a6b7d4d5 100644 --- a/Extras/InverseDynamics/invdyn_bullet_comparison.hpp +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp @@ -1,12 +1,13 @@ #ifndef INVDYN_BULLET_COMPARISON_HPP #define INVDYN_BULLET_COMPARISON_HPP +#include "BulletInverseDynamics/IDConfig.hpp" + class btMultiBody; class btVector3; namespace btInverseDynamics { class MultiBodyTree; -class vecx; /// this function compares the forward dynamics computations implemented in btMultiBody to /// the inverse dynamics implementation in MultiBodyTree. This is done in three steps diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index dadbd8235..8e6577912 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -2,8 +2,17 @@ /// such as choice of linear algebra library and underlying scalar type #ifndef IDCONFIG_HPP_ #define IDCONFIG_HPP_ + +// If true, enable jacobian calculations. +// This adds a 3xN matrix to every body, + 2 3-Vectors. +// so it is not advised for large systems if it is not absolutely necessary. +// Also, this is not required for standard inverse dynamics calculations. +// Will only work with vector math libraries that support 3xN matrices. +#define BT_ID_WITH_JACOBIANS + // If we have a custom configuration, compile without using other parts of bullet. #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +#include #define BT_ID_WO_BULLET #define BT_ID_POW(a,b) std::pow(a,b) #define BT_ID_SNPRINTF snprintf @@ -25,7 +34,7 @@ #define INVDYN_INCLUDE_HELPER_2(x) #x #define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) #include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) -#ifndef btInverseDynamics +#ifndef btInverseDynamics #error "custom inverse dynamics config, but no custom namespace defined" #endif diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp index a83ee8e3f..cbd7e8a9c 100644 --- a/src/BulletInverseDynamics/IDConfigEigen.hpp +++ b/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -21,20 +21,7 @@ struct idArray { typedef std::vector::size_type idArrayIdx; // default to standard malloc/free #include -#define idMalloc ::malloc - -#define idFree ::free -// currently not aligned at all... -#define ID_DECLARE_ALIGNED_ALLOCATOR() \ - inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete(void* ptr) { idFree(ptr); } \ - inline void* operator new(std::size_t, void* ptr) { return ptr; } \ - inline void operator delete(void*, void*) {} \ - inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete[](void* ptr) { idFree(ptr); } \ - inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ - inline void operator delete[](void*, void*) {} - +#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Note on interfaces: // Eigen::Matrix has data(), to get c-array storage // HOWEVER: default storage is column-major! diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index 7ce055fdf..03452ca0c 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -55,6 +55,60 @@ idScalar maxAbs(const vec3 &v) { return result; } +#if (defined BT_ID_HAVE_MAT3X) +idScalar maxAbsMat3x(const mat3x &m) { + // only used for tests -- so just loop here for portability + idScalar result = 0.0; + for (idArrayIdx col = 0; col < m.cols(); col++) { + for (idArrayIdx row = 0; row < 3; row++) { + result = BT_ID_MAX(result, std::fabs(m(row, col))); + } + } + return result; +} + +void mul(const mat33 &a, const mat3x &b, mat3x *result) { + if (b.cols() != result->cols()) { + error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast(b.cols()), static_cast(result->cols())); + abort(); + } + + for (idArrayIdx col = 0; col < b.cols(); col++) { + const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col); + const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col); + const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col); + setMat3xElem(0, col, x, result); + setMat3xElem(1, col, y, result); + setMat3xElem(2, col, z, result); + } +} +void add(const mat3x &a, const mat3x &b, mat3x *result) { + if (a.cols() != b.cols()) { + error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast(a.cols()), static_cast(b.cols())); + abort(); + } + for (idArrayIdx col = 0; col < b.cols(); col++) { + for (idArrayIdx row = 0; row < 3; row++) { + setMat3xElem(row, col, a(row, col) + b(row, col), result); + } + } +} +void sub(const mat3x &a, const mat3x &b, mat3x *result) { + if (a.cols() != b.cols()) { + error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast(a.cols()), static_cast(b.cols())); + abort(); + } + for (idArrayIdx col = 0; col < b.cols(); col++) { + for (idArrayIdx row = 0; row < 3; row++) { + setMat3xElem(row, col, a(row, col) - b(row, col), result); + } + } +} +#endif + mat33 transformX(const idScalar &alpha) { mat33 T; const idScalar cos_alpha = std::cos(alpha); diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp index f7526b500..63699712a 100644 --- a/src/BulletInverseDynamics/IDMath.hpp +++ b/src/BulletInverseDynamics/IDMath.hpp @@ -19,6 +19,16 @@ idScalar maxAbs(const vecx& v); /// return maximum absolute value idScalar maxAbs(const vec3& v); #endif //ID_LINEAR_MATH_USE_EIGEN + +#if (defined BT_ID_HAVE_MAT3X) +idScalar maxAbsMat3x(const mat3x& m); +void setZero(mat3x&m); +// define math functions on mat3x here to avoid allocations in operators. +void mul(const mat33&a, const mat3x&b, mat3x* result); +void add(const mat3x&a, const mat3x&b, mat3x* result); +void sub(const mat3x&a, const mat3x&b, mat3x* result); +#endif + /// get offset vector & transform matrix from DH parameters /// TODO: add documentation void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T); diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index f5488b62a..4235f138d 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -61,6 +61,18 @@ int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_a return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); } +int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const { + return m_impl->getParentRParentBodyRef(body_index, r); +} + +int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const { + return m_impl->getBodyTParentRef(body_index, T); +} + +int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const { + return m_impl->getBodyAxisOfMotion(body_index, axis); +} + void MultiBodyTree::printTree() { m_impl->printTree(); } void MultiBodyTree::printTreeData() { m_impl->printTreeData(); } @@ -96,9 +108,106 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati } return 0; } + int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { return calculateMassMatrix(q, true, true, true, mass_matrix); } + + + +int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) { + vec3 world_gravity(m_impl->m_world_gravity); + // temporarily set gravity to zero, to ensure we get the actual accelerations + setZero(m_impl->m_world_gravity); + + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, dot_u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) { + error_message("error in kinematics calculation\n"); + return -1; + } + + m_impl->m_world_gravity=world_gravity; + return 0; +} + + +int MultiBodyTree::calculatePositionKinematics(const vecx& q) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, q, q, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in kinematics calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in kinematics calculation\n"); + return -1; + } + return 0; +} + + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) +int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in jacobian calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateJacobians(const vecx& q){ + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, q, + MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) { + error_message("error in jacobian calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { + return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u); +} + +int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const { + return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u); +} + +int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const { + return m_impl->getBodyJacobianTrans(body_index,world_jac_trans); +} + +int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const { + return m_impl->getBodyJacobianRot(body_index,world_jac_rot); +} + + +#endif + int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type, const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, const vec3 &body_axis_of_motion_, idScalar mass, @@ -186,6 +295,11 @@ int MultiBodyTree::finalize() { const int &num_bodies = m_init_cache->numBodies(); const int &num_dofs = m_init_cache->numDoFs(); + if(num_dofs<=0) { + error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); + //return -1; + } + // 1 allocate internal MultiBody structure m_impl = new MultiBodyImpl(num_bodies, num_dofs); diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp index d33e60e12..d235aa6e7 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -51,6 +51,7 @@ enum JointType { /// - gears and motor inertia class MultiBodyTree { public: + ID_DECLARE_ALIGNED_ALLOCATOR(); /// The contructor. /// Initialization & allocation is via addBody and buildSystem calls. MultiBodyTree(); @@ -117,7 +118,10 @@ public: void printTree(); /// print tree data to stdout void printTreeData(); - /// calculate joint forces for given generalized state & derivatives + /// Calculate joint forces for given generalized state & derivatives. + /// This also updates kinematic terms computed in calculateKinematics. + /// If gravity is not set to zero, acceleration terms will contain + /// gravitational acceleration. /// @param q generalized coordinates /// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u) /// @param dot_u time derivative of u @@ -148,6 +152,31 @@ public: /// @return -1 on error, 0 on success int calculateMassMatrix(const vecx& q, matxx* mass_matrix); + + /// Calculates kinematics also calculated in calculateInverseDynamics, + /// but not dynamics. + /// This function ensures that correct accelerations are computed that do not + /// contain gravitational acceleration terms. + /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations) + int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u); + /// Calculate position kinematics + int calculatePositionKinematics(const vecx& q); + /// Calculate position and velocity kinematics + int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u); + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components + /// d(Jacobian)/dt*u + /// This function assumes that calculateInverseDynamics was called, or calculateKinematics, + /// or calculatePositionAndVelocityKinematics + int calculateJacobians(const vecx& q, const vecx& u); + /// Calculate Jacobians (dvel/du) + /// This function assumes that calculateInverseDynamics was called, or + /// one of the calculateKineamtics functions + int calculateJacobians(const vecx& q); +#endif // BT_ID_HAVE_MAT3X + + /// set gravitational acceleration /// the default is [0;0;-9.8] in the world frame /// @param gravity the gravitational acceleration in world frame @@ -200,9 +229,20 @@ public: /// @param world_origin pointer for return data /// @return 0 on success, -1 on error int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + // get translational jacobian, in world frame (dworld_velocity/du) + int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const; + // get rotational jacobian, in world frame (dworld_omega/du) + int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; + // get product of translational jacobian derivative * generatlized velocities + int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const; + // get product of rotational jacobian derivative * generatlized velocities + int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; +#endif // BT_ID_HAVE_MAT3X + /// returns the (internal) index of body - /// @param body_index is the index of a body (internal: TODO: fix/clarify - /// indexing!) + /// @param body_index is the index of a body /// @param parent_index pointer to where parent index will be stored /// @return 0 on success, -1 on error int getParentIndex(const int body_index, int* parent_index) const; @@ -216,6 +256,21 @@ public: /// @param joint_type string naming the corresponding joint type /// @return 0 on success, -1 on failure int getJointTypeStr(const int body_index, const char** joint_type) const; + /// get offset translation to parent body (see addBody) + /// @param body_index index of the body + /// @param r the offset translation (see above) + /// @return 0 on success, -1 on failure + int getParentRParentBodyRef(const int body_index, vec3* r) const; + /// get offset rotation to parent body (see addBody) + /// @param body_index index of the body + /// @param T the transform (see above) + /// @return 0 on success, -1 on failure + int getBodyTParentRef(const int body_index, mat33* T) const; + /// get axis of motion (see addBody) + /// @param body_index index of the body + /// @param axis the axis (see above) + /// @return 0 on success, -1 on failure + int getBodyAxisOfMotion(const int body_index, vec3* axis) const; /// get offset for degrees of freedom of this body into the q-vector /// @param body_index index of the body /// @param q_offset offset the q vector diff --git a/src/BulletInverseDynamics/details/IDEigenInterface.hpp b/src/BulletInverseDynamics/details/IDEigenInterface.hpp index ec6136aa5..836395cea 100644 --- a/src/BulletInverseDynamics/details/IDEigenInterface.hpp +++ b/src/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -2,16 +2,35 @@ #define INVDYNEIGENINTERFACE_HPP_ #include "../IDConfig.hpp" namespace btInverseDynamics { + +#define BT_ID_HAVE_MAT3X + #ifdef BT_USE_DOUBLE_PRECISION -typedef Eigen::VectorXd vecx; -typedef Eigen::Vector3d vec3; -typedef Eigen::Matrix3d mat33; -typedef Eigen::MatrixXd matxx; +typedef Eigen::Matrix vecx; +typedef Eigen::Matrix vec3; +typedef Eigen::Matrix mat33; +typedef Eigen::Matrix matxx; +typedef Eigen::Matrix mat3x; #else -typedef Eigen::VectorXf vecx; -typedef Eigen::Vector3f vec3; -typedef Eigen::Matrix3f mat33; -typedef Eigen::MatrixXf matxx; -#endif // +typedef Eigen::Matrix vecx; +typedef Eigen::Matrix vec3; +typedef Eigen::Matrix mat33; +typedef Eigen::Matrix matxx; +typedef Eigen::Matrix mat3x; +#endif + +inline void resize(mat3x &m, Eigen::Index size) { + m.resize(3, size); + m.setZero(); +} + +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ + (*m)(row, col) = val; +} + +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ + (*m)(row, col) = val; +} + } #endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index 53f88c4b7..cbe6e5a96 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -8,13 +8,13 @@ #include "../../LinearMath/btMatrix3x3.h" #include "../../LinearMath/btVector3.h" #include "../../LinearMath/btMatrixX.h" +#define BT_ID_HAVE_MAT3X namespace btInverseDynamics { class vec3; class vecx; class mat33; typedef btMatrixX matxx; -typedef btVectorX vecxx; class vec3 : public btVector3 { public: @@ -47,11 +47,11 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s) inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -class vecx : public vecxx { +class vecx : public btVectorX { public: - vecx(int size) : vecxx(size) {} - const vecx& operator=(const vecxx& rhs) { - *static_cast(this) = rhs; + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) { + *static_cast(this) = rhs; return *this; } @@ -108,6 +108,65 @@ inline vecx operator/(const vecx& a, const idScalar& s) { return result; } + +// use btMatrixX to implement 3xX matrix +class mat3x : public matxx { +public: + mat3x(){} + mat3x(const mat3x&rhs) { + matxx::resize(rhs.rows(), rhs.cols()); + *this = rhs; + } + mat3x(int rows, int cols): matxx(3,cols) { + } + void operator=(const mat3x& rhs) { + if (m_cols != rhs.m_cols) { + error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for(int i=0;isetElem(row, col, val); +} + +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ + m->setElem(row, col, val); +} + } #endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp index de15e9ed4..4d3f6c87e 100644 --- a/src/BulletInverseDynamics/details/IDMatVec.hpp +++ b/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -5,12 +5,14 @@ #include #include "../IDConfig.hpp" +#define BT_ID_HAVE_MAT3X namespace btInverseDynamics { class vec3; class vecx; class mat33; class matxx; +class mat3x; /// This is a very basic implementation to enable stand-alone use of the library. /// The implementation is not really optimized and misses many features that you would @@ -85,13 +87,17 @@ private: class matxx { public: + matxx() { + m_data = 0x0; + m_cols=0; + m_rows=0; + } matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); } ~matxx() { idFree(m_data); } - const matxx& operator=(const matxx& rhs); - idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; } - const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; } + idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; } const int& rows() const { return m_rows; } const int& cols() const { return m_cols; } @@ -101,6 +107,61 @@ private: idScalar* m_data; }; +class mat3x { +public: + mat3x() { + m_data = 0x0; + m_cols=0; + } + mat3x(const mat3x&rhs) { + m_cols=rhs.m_cols; + allocate(); + *this = rhs; + } + mat3x(int rows, int cols): m_cols(cols) { + allocate(); + }; + void operator=(const mat3x& rhs) { + if (m_cols != rhs.m_cols) { + error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for(int i=0;i<3*m_cols;i++) { + m_data[i] = rhs.m_data[i]; + } + } + + ~mat3x() { + free(); + } + idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; } + int rows() const { return m_rows; } + const int& cols() const { return m_cols; } + void resize(int rows, int cols) { + m_cols=cols; + free(); + allocate(); + } + void setZero() { + memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols); + } + // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead +private: + void allocate(){m_data = static_cast(idMalloc(sizeof(idScalar) * m_rows * m_cols));} + void free() { idFree(m_data);} + enum {m_rows=3}; + int m_cols; + idScalar* m_data; +}; + +inline void resize(mat3x &m, idArrayIdx size) { + m.resize(3, size); + m.setZero(); +} + +////////////////////////////////////////////////// +// Implementations inline const vec3& vec3::operator=(const vec3& rhs) { if (&rhs != this) { memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); @@ -324,5 +385,31 @@ inline vecx operator/(const vecx& a, const idScalar& s) { return result; } + +inline vec3 operator*(const mat3x& a, const vecx& b) { + vec3 result; + if (a.cols() != b.size()) { + error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); + abort(); + } + result(0)=0.0; + result(1)=0.0; + result(2)=0.0; + for(int i=0;i(dot_u.size()), static_cast(joint_forces->size())); return -1; } - // 1. update relative kinematics - // 1.1 for revolute - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - mat33 T; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); - body.m_body_T_parent = T * body.m_body_T_parent_ref; - - // body.m_parent_r_parent_body= fixed - body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); - // body.m_parent_dot_r_rel = fixed; - // NOTE: this assumes that Jac_JR is constant, which is true for revolute - // joints, but not in the general case (eg, slider-crank type mechanisms) - body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); - // body.m_parent_ddot_r_rel = fixed; - } - // 1.2 for prismatic - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - // body.m_parent_omega_rel = 0; - body.m_parent_vel_rel = - body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); - // body.parent_dot_omega_rel = 0; - // NOTE: this assumes that Jac_JT is constant, which is true for - // prismatic joints, but not in the general case - body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - - body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); - body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); - body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); - - body.m_parent_vel_rel(0) = u(body.m_q_index + 3); - body.m_parent_vel_rel(1) = u(body.m_q_index + 4); - body.m_parent_vel_rel(2) = u(body.m_q_index + 5); - - body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); - body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); - body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); - - body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); - body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); - body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); - - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; - body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; - } - - // 3. absolute kinematic quantities & dynamic quantities - // NOTE: this should be optimized by specializing for different body types - // (e.g., relative rotation is always zero for prismatic joints, etc.) - - // calculations for root body - { - RigidBody &body = m_body_list[0]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) - // not required right now, added here for debugging purposes - body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_body_T_world = body.m_body_T_parent; - - // 3.2 update absolute velocities - body.m_body_ang_vel = body.m_body_ang_vel_rel; - body.m_body_vel = body.m_parent_vel_rel; - - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = body.m_body_ang_acc_rel; - body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; - // add gravitational acceleration to root body - // this is an efficient way to add gravitational terms, - // but it does mean that the kinematics are no longer - // correct at the acceleration level - // NOTE: To get correct acceleration kinematics, just set world_gravity to zero - body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; - } - - for (idArrayIdx i = 1; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - RigidBody &parent = m_body_list[m_parent_index[i]]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) not required right now added here for debugging purposes - body.m_body_pos = - body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); - body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; - - // 3.2 update absolute velocities - body.m_body_ang_vel = - body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; - - body.m_body_vel = - body.m_body_T_parent * - (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + - body.m_parent_vel_rel); - - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = - body.m_body_T_parent * parent.m_body_ang_acc - - body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + - body.m_body_ang_acc_rel; - body.m_body_acc = - body.m_body_T_parent * - (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + - parent.m_body_ang_vel.cross( - parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + - 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); - } - + // 1. relative kinematics + if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { + error_message("error in calculateKinematics\n"); + return -1; + } + // 2. update contributions to equations of motion for every body. for (idArrayIdx i = 0; i < m_body_list.size(); i++) { RigidBody &body = m_body_list[i]; // 3.4 update dynamic terms (rate of change of angular & linear momentum) @@ -356,7 +259,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const body.m_body_force_user; } - // 4. calculate full set of forces at parent joint + // 3. calculate full set of forces at parent joint // (not directly calculating the joint force along the free direction // simplifies inclusion of fixed joints. // An alternative would be to fuse bodies in a pre-processing step, @@ -416,6 +319,239 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const return 0; } +int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u, + const KinUpdateType type) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size()), + static_cast(dot_u.size())); + return -1; + } + if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { + error_message("invalid type %d\n", type); + return -1; + } + + // 1. update relative kinematics + // 1.1 for revolute + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + mat33 T; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + if(type >= POSITION_VELOCITY) { + body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); + } + } + // 1.2 for prismatic + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + if(type >= POSITION_VELOCITY) { + body.m_parent_vel_rel = + body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); + } + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + + if(type >= POSITION_VELOCITY) { + body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); + body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); + body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); + + body.m_parent_vel_rel(0) = u(body.m_q_index + 3); + body.m_parent_vel_rel(1) = u(body.m_q_index + 4); + body.m_parent_vel_rel(2) = u(body.m_q_index + 5); + + body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); + body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); + body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); + + body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); + body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); + body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); + + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } + } + + // 2. absolute kinematic quantities (vector valued) + // NOTE: this should be optimized by specializing for different body types + // (e.g., relative rotation is always zero for prismatic joints, etc.) + + // calculations for root body + { + RigidBody &body = m_body_list[0]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) + // not required right now, added here for debugging purposes + body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_body_T_world = body.m_body_T_parent; + + if(type >= POSITION_VELOCITY) { + // 3.2 update absolute velocities + body.m_body_ang_vel = body.m_body_ang_vel_rel; + body.m_body_vel = body.m_parent_vel_rel; + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = body.m_body_ang_acc_rel; + body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; + // add gravitational acceleration to root body + // this is an efficient way to add gravitational terms, + // but it does mean that the kinematics are no longer + // correct at the acceleration level + // NOTE: To get correct acceleration kinematics, just set world_gravity to zero + body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; + } + } + + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + // 2.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) not required right now added here for debugging purposes + body.m_body_pos = + body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); + body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; + + if(type >= POSITION_VELOCITY) { + // 2.2 update absolute velocities + body.m_body_ang_vel = + body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; + + body.m_body_vel = + body.m_body_T_parent * + (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + + body.m_parent_vel_rel); + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + // 2.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = + body.m_body_T_parent * parent.m_body_ang_acc - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + + body.m_body_ang_acc_rel; + body.m_body_acc = + body.m_body_T_parent * + (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); + } + } + + return 0; +} + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + +void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) { + const int& idx=body.m_q_index; + switch(body.m_joint_type) { + case FIXED: + break; + case REVOLUTE: + setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R); + setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R); + setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R); + break; + case PRISMATIC: + setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0) + +body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1) + +body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2), + &body.m_body_Jac_T); + setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0) + +body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1) + +body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2), + &body.m_body_Jac_T); + setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0) + +body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1) + +body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2), + &body.m_body_Jac_T); + break; + case FLOATING: + setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R); + setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R); + setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R); + // body_Jac_T = body_T_parent.transpose(); + setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T); + setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T); + setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T); + + setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T); + setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T); + setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T); + + setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T); + setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T); + setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T); + + break; + } +} + +int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size())); + return -1; + } + if(type != POSITION_ONLY && type != POSITION_VELOCITY) { + error_message("invalid type %d\n", type); + return -1; + } + + addRelativeJacobianComponent(m_body_list[0]); + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + + mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R); + body.m_body_Jac_T = parent.m_body_Jac_T; + mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x); + sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T); + + addRelativeJacobianComponent(body); + mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T); + + if(type >= POSITION_VELOCITY) { + body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel); + body.m_body_dot_Jac_T_u = body.m_body_T_parent * + (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel)); + } + } + return 0; +} +#endif + static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { switch (dof) { // rotational part @@ -487,13 +623,6 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // This implementation, however, handles branched systems and uses a formulation centered // on the origin of the body-fixed frame to avoid re-computing various quantities at the com. -// Macro for setting matrix elements depending on underlying math library. -#ifdef ID_LINEAR_MATH_USE_BULLET -#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val) -#else -#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val -#endif - if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || mass_matrix->cols() != m_num_dofs) { error_message("Dimension error. System has %d DOFs,\n" @@ -507,7 +636,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool if (initialize_matrix) { for (int i = 0; i < m_num_dofs; i++) { for (int j = 0; j < m_num_dofs; j++) { - setMassMatrixElem(i, j, 0.0); + setMatxxElem(i, j, 0.0, mass_matrix); } } } @@ -597,7 +726,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); vec3 body_eom_trans = body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); - setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans)); + setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix); // rest of the mass matrix column upwards { @@ -609,7 +738,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMassMatrixElem(col, row, Mrc); + setMatxxElem(col, row, Mrc, mass_matrix); } // 2. ancestor dofs int child_idx = i; @@ -634,7 +763,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); } const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMassMatrixElem(col, row, Mrc); + setMatxxElem(col, row, Mrc, mass_matrix); } child_idx = parent_idx; @@ -647,12 +776,10 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool if (set_lower_triangular_matrix) { for (int col = 0; col < m_num_dofs; col++) { for (int row = 0; row < col; row++) { - setMassMatrixElem(row, col, (*mass_matrix)(col, row)); + setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); } } } - -#undef setMassMatrixElem return 0; } @@ -779,6 +906,32 @@ int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, return 0; } +int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *r=m_body_list[body_index].m_parent_pos_parent_body_ref; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *T=m_body_list[body_index].m_body_T_parent_ref; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + if(m_body_list[body_index].m_joint_type == REVOLUTE) { + *axis = m_body_list[body_index].m_Jac_JR; + return 0; + } + if(m_body_list[body_index].m_joint_type == PRISMATIC) { + *axis = m_body_list[body_index].m_Jac_JT; + return 0; + } + setZero(*axis); + return 0; +} + int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { CHECK_IF_BODY_INDEX_IS_VALID(body_index); *q_index = m_body_list[body_index].m_q_index; @@ -841,4 +994,34 @@ int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 return 0; } +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) +int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot); + return 0; +} + +#endif } diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 7787a4665..3efe9d049 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -110,6 +110,19 @@ struct RigidBody { vec3 m_body_subtree_mass_com; /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame mat33 m_body_subtree_I_body; + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + /// translational jacobian in body-fixed frame d(m_body_vel)/du + mat3x m_body_Jac_T; + /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du + mat3x m_body_Jac_R; + /// components of linear acceleration depending on u + /// (same as is d(m_Jac_T)/dt*u) + vec3 m_body_dot_Jac_T_u; + /// components of angular acceleration depending on u + /// (same as is d(m_Jac_T)/dt*u) + vec3 m_body_dot_Jac_R_u; +#endif }; /// The MBS implements a tree structured multibody system @@ -119,6 +132,12 @@ class MultiBodyTree::MultiBodyImpl { public: ID_DECLARE_ALIGNED_ALLOCATOR(); + enum KinUpdateType { + POSITION_ONLY, + POSITION_VELOCITY, + POSITION_VELOCITY_ACCELERATION + }; + /// constructor /// @param num_bodies the number of bodies in the system /// @param num_dofs number of degrees of freedom in the system @@ -131,6 +150,25 @@ public: int calculateMassMatrix(const vecx& q, const bool update_kinematics, const bool initialize_matrix, const bool set_lower_triangular_matrix, matxx* mass_matrix); + /// calculate kinematics (vector quantities) + /// Depending on type, update positions only, positions & velocities, or positions, velocities + /// and accelerations. + int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type); +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + /// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms. + int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type); + /// \copydoc MultiBodyTree::getBodyDotJacobianTransU + int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ; + /// \copydoc MultiBodyTree::getBodyDotJacobianRotU + int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; + /// \copydoc MultiBodyTree::getBodyJacobianTrans + int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ; + /// \copydoc MultiBodyTree::getBodyJacobianRot + int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; + /// Add relative Jacobian component from motion relative to parent body + /// @param body the body to add the Jacobian component for + void addRelativeJacobianComponent(RigidBody&body); +#endif /// generate additional index sets from the parent_index array /// @return -1 on error, 0 on success int generateIndexSets(); @@ -152,6 +190,12 @@ public: int getJointType(const int body_index, JointType* joint_type) const; /// \copydoc MultiBodyTree::getJointTypeStr int getJointTypeStr(const int body_index, const char** joint_type) const; + /// \copydoc MultiBodyTree::getParentRParentBodyRef + int getParentRParentBodyRef(const int body_index, vec3* r) const; + /// \copydoc MultiBodyTree::getBodyTParentRef + int getBodyTParentRef(const int body_index, mat33* T) const; + /// \copydoc MultiBodyTree::getBodyAxisOfMotion + int getBodyAxisOfMotion(const int body_index, vec3* axis) const; /// \copydoc MultiBodyTree:getDoFOffset int getDoFOffset(const int body_index, int* q_index) const; /// \copydoc MultiBodyTree::getBodyOrigin @@ -231,6 +275,9 @@ private: idArray::type m_user_int; // a user-provided pointer idArray::type m_user_ptr; +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + mat3x m_m3x; +#endif }; } #endif diff --git a/test/InverseDynamics/CMakeLists.txt b/test/InverseDynamics/CMakeLists.txt index 89d70c06a..e31ea6e12 100644 --- a/test/InverseDynamics/CMakeLists.txt +++ b/test/InverseDynamics/CMakeLists.txt @@ -20,14 +20,112 @@ IF (NOT WIN32) ENDIF() - ADD_EXECUTABLE(Test_BulletInverseDynamics - test_invdyn_kinematics.cpp + ADD_EXECUTABLE(Test_BulletInverseDynamicsJacobian + test_invdyn_jacobian.cpp ) +ADD_TEST(Test_BulletInverseDynamicsJacobian_PASS Test_BulletInverseDynamicsJacobian) + +IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + +INCLUDE_DIRECTORIES( + . + ../../src + ../gtest-1.7.0/include + ../../Extras/InverseDynamics +) + + +#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1) +ADD_DEFINITIONS(-D_VARIADIC_MAX=10) + +LINK_LIBRARIES( +BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest +) +IF (NOT WIN32) + LINK_LIBRARIES( pthread ) +ENDIF() + + + ADD_EXECUTABLE(Test_BulletInverseForwardDynamics + test_invdyn_bullet.cpp + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h + ../../examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.h + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.h + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/Utils/b3Clock.cpp + ../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp + ../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp + ../../Extras/Serialize/BulletFileLoader/bChunk.cpp + ../../Extras/Serialize/BulletFileLoader/bFile.cpp + ../../Extras/Serialize/BulletFileLoader/bDNA.cpp + ../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp + ../../examples/Importers/ImportURDFDemo/URDFImporterInterface.h + ../../examples/Importers/ImportURDFDemo/URDFJointTypes.h + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportSTLDemo/ImportSTLSetup.h + ../../examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportColladaDemo/ColladaGraphicsInstance.h + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp + + ) + +ADD_TEST(Test_BulletInverseForwardDynamics_PASS Test_BulletInverseForwardDynamics) + +IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + +INCLUDE_DIRECTORIES( + . + ../../src + ../gtest-1.7.0/include + ../../Extras/InverseDynamics +) + + +#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1) +ADD_DEFINITIONS(-D_VARIADIC_MAX=10) + +LINK_LIBRARIES( + BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest +) +IF (NOT WIN32) + LINK_LIBRARIES( pthread ) +ENDIF() + + + ADD_EXECUTABLE(Test_BulletInverseDynamics + test_invdyn_kinematics.cpp + ) + ADD_TEST(Test_BulletInverseDynamics_PASS Test_BulletInverseDynamics) IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) - SET_TARGET_PROPERTIES(Test_Collision PROPERTIES DEBUG_POSTFIX "_Debug") - SET_TARGET_PROPERTIES(Test_Collision PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") - SET_TARGET_PROPERTIES(Test_Collision PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") + SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua index 9d6770347..1892a0206 100644 --- a/test/InverseDynamics/premake4.lua +++ b/test/InverseDynamics/premake4.lua @@ -37,7 +37,8 @@ - project "Test_InverseForwardDynamics" + project "Test_InverseDynamicsJacobian" + kind "ConsoleApp" @@ -45,6 +46,38 @@ + 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_jacobian.cpp", + } + + if os.is("Linux") then + links {"pthread"} + end + + project "Test_InverseForwardDynamics" + kind "ConsoleApp" +-- defines { } + + + includedirs { ".", diff --git a/test/InverseDynamics/test_invdyn_bullet.cpp b/test/InverseDynamics/test_invdyn_bullet.cpp index 1ac095666..63597a562 100644 --- a/test/InverseDynamics/test_invdyn_bullet.cpp +++ b/test/InverseDynamics/test_invdyn_bullet.cpp @@ -16,12 +16,12 @@ #include #include #include -#include <../CommonInterfaces/CommonGUIHelperInterface.h> #include -#include <../Importers/ImportURDFDemo/BulletUrdfImporter.h> -#include <../Importers/ImportURDFDemo/URDF2Bullet.h> -#include <../Importers/ImportURDFDemo/MyMultiBodyCreator.h> -#include <../Importers/ImportURDFDemo/URDF2Bullet.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 #include diff --git a/test/InverseDynamics/test_invdyn_jacobian.cpp b/test/InverseDynamics/test_invdyn_jacobian.cpp new file mode 100644 index 000000000..01907b2b2 --- /dev/null +++ b/test/InverseDynamics/test_invdyn_jacobian.cpp @@ -0,0 +1,326 @@ +// Test of kinematic consistency: check if finite differences of velocities, accelerations +// match positions + +#include +#include +#include +#include + +#include + +#include "Bullet3Common/b3Random.h" + +#include "CloneTreeCreator.hpp" +#include "CoilCreator.hpp" +#include "DillCreator.hpp" +#include "RandomTreeCreator.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "MultiBodyTreeDebugGraph.hpp" + +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 +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& lhs, const ptr& rhs) { return rhs.m_p == lhs.m_p; } + friend bool operator!=(const ptr& lhs, const ptr& rhs) { return !(rhs.m_p == lhs.m_p); +} + +private: + 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 tree1(CreateMultiBodyTree(creator)); + ASSERT_TRUE(0x0 != tree1); + CloneTreeCreator clone(tree1.get()); + // tree2 is used to compute dot(Jacobian)*u using the calculateJacobian function + ptr tree2(CreateMultiBodyTree(clone)); + ASSERT_TRUE(0x0 != tree2); + + 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); + + 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); + } + + EXPECT_EQ(0, tree1->calculateKinematics(q, u, zero)); + EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u)); + EXPECT_EQ(0, tree2->calculateJacobians(q, u)); + + for (size_t 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; + } + + 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 tree1(CreateMultiBodyTree(creator)); + ASSERT_TRUE(0x0 != tree1); + // tree2 is used to compute the Jacobians using the calculateJacobian function + CloneTreeCreator clone(tree1.get()); + ptr tree2(CreateMultiBodyTree(clone)); + ASSERT_TRUE(0x0 != tree2); + + const int ndofs = tree1->numDoFs(); + const int nbodies = tree1->numBodies(); + + if (ndofs <= 0) { + *max_error = 0; + return; + } + + vecx q(ndofs); + vecx zero(ndofs); + setZero(zero); + vecx one(ndofs); + + 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); + } + + EXPECT_EQ(0, tree2->calculatePositionKinematics(q)); + EXPECT_EQ(0, tree2->calculateJacobians(q)); + + for (size_t 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 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); + + 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 tree1(CreateMultiBodyTree(creator)); + ASSERT_TRUE(0x0 != tree1); + // tree2 is used to compute the velocities via jacobians + CloneTreeCreator clone(tree1.get()); + ptr tree2(CreateMultiBodyTree(clone)); + ASSERT_TRUE(0x0 != tree2); + + const int ndofs = tree1->numDoFs(); + const int nbodies = tree1->numBodies(); + + if (ndofs <= 0) { + *max_error = 0; + return; + } + + vecx q(ndofs); + vecx u(ndofs); + + 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); + } + + EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, u)); + EXPECT_EQ(0, tree2->calculatePositionKinematics(q)); + EXPECT_EQ(0, tree2->calculateJacobians(q)); + + for (size_t 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; + + 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; +#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); + } +} + +// 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) { + b3Srand(1234); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}