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(); +}