diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/details')
7 files changed, 984 insertions, 619 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp index 836395cea2..fe4f102513 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -1,8 +1,8 @@ #ifndef INVDYNEIGENINTERFACE_HPP_ #define INVDYNEIGENINTERFACE_HPP_ #include "../IDConfig.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ #define BT_ID_HAVE_MAT3X #ifdef BT_USE_DOUBLE_PRECISION @@ -19,18 +19,21 @@ typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> m typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x; #endif -inline void resize(mat3x &m, Eigen::Index size) { - m.resize(3, size); - m.setZero(); +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 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; +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x *m) +{ + (*m)(row, col) = val; } -} +} // namespace btInverseDynamics #endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp index c179daeec6..0c398a3727 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -10,32 +10,37 @@ #include "../../LinearMath/btMatrixX.h" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; typedef btMatrixX<idScalar> matxx; -class vec3 : public btVector3 { +class vec3 : public btVector3 +{ public: vec3() : btVector3() {} vec3(const btVector3& btv) { *this = btv; } idScalar& operator()(int i) { return (*this)[i]; } const idScalar& operator()(int i) const { return (*this)[i]; } int size() const { return 3; } - const vec3& operator=(const btVector3& rhs) { + const vec3& operator=(const btVector3& rhs) + { *static_cast<btVector3*>(this) = rhs; return *this; } }; -class mat33 : public btMatrix3x3 { +class mat33 : public btMatrix3x3 +{ public: mat33() : btMatrix3x3() {} mat33(const btMatrix3x3& btm) { *this = btm; } idScalar& operator()(int i, int j) { return (*this)[i][j]; } const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } - const mat33& operator=(const btMatrix3x3& rhs) { + const mat33& operator=(const btMatrix3x3& rhs) + { *static_cast<btMatrix3x3*>(this) = rhs; return *this; } @@ -47,10 +52,12 @@ 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 btVectorX<idScalar> { +class vecx : public btVectorX<idScalar> +{ public: vecx(int size) : btVectorX<idScalar>(size) {} - const vecx& operator=(const btVectorX<idScalar>& rhs) { + const vecx& operator=(const btVectorX<idScalar>& rhs) + { *static_cast<btVectorX<idScalar>*>(this) = rhs; return *this; } @@ -66,43 +73,53 @@ public: friend vecx operator/(const vecx& a, const idScalar& s); }; -inline vecx operator*(const vecx& a, const idScalar& s) { +inline vecx operator*(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) * s; } return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) { +inline vecx operator+(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result(i) = a(i) + b(i); } return result; } -inline vecx operator-(const vecx& a, const vecx& b) { +inline vecx operator-(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result(i) = a(i) - b(i); } return result; } -inline vecx operator/(const vecx& a, const idScalar& s) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) / s; } @@ -110,63 +127,76 @@ inline vecx operator/(const vecx& a, const idScalar& s) { } // use btMatrixX to implement 3xX matrix -class mat3x : public matxx { +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) { - bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); - abort(); + 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) + { + bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for (int i = 0; i < rows(); i++) + { + for (int k = 0; k < cols(); k++) + { + setElem(i, k, rhs(i, k)); + } + } + } + void setZero() + { + matxx::setZero(); } - for(int i=0;i<rows();i++) { - for(int k=0;k<cols();k++) { - setElem(i,k,rhs(i,k)); - } - } - } - void setZero() { - matxx::setZero(); - } }; - -inline vec3 operator*(const mat3x& a, const vecx& b) { - vec3 result; - if (a.cols() != b.size()) { - bt_id_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<b.size();i++) { - for(int k=0;k<3;k++) { - result(k)+=a(k,i)*b(i); - } - } - return result; +inline vec3 operator*(const mat3x& a, const vecx& b) +{ + vec3 result; + if (a.cols() != b.size()) + { + bt_id_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 < b.size(); i++) + { + for (int k = 0; k < 3; k++) + { + result(k) += a(k, i) * b(i); + } + } + return result; } - -inline void resize(mat3x &m, idArrayIdx size) { - m.resize(3, size); - m.setZero(); +inline void resize(mat3x& m, idArrayIdx size) +{ + m.resize(3, size); + m.setZero(); } -inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ - m->setElem(row, col, val); +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m) +{ + m->setElem(row, col, val); } -inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ - m->setElem(row, col, val); +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) +{ + m->setElem(row, col, val); } -} +} // namespace btInverseDynamics #endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp index c89db5e123..1c786095e7 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp @@ -7,7 +7,8 @@ #include "../IDConfig.hpp" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; @@ -17,7 +18,8 @@ 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 /// want from a "fully featured" linear math library. -class vec3 { +class vec3 +{ public: idScalar& operator()(int i) { return m_data[i]; } const idScalar& operator()(int i) const { return m_data[i]; } @@ -40,7 +42,8 @@ private: idScalar m_data[3]; }; -class mat33 { +class mat33 +{ public: idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } @@ -62,9 +65,11 @@ private: idScalar m_data[9]; }; -class vecx { +class vecx +{ public: - vecx(int size) : m_size(size) { + vecx(int size) : m_size(size) + { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size)); } ~vecx() { idFree(m_data); } @@ -85,14 +90,17 @@ private: idScalar* m_data; }; -class matxx { +class matxx +{ public: - matxx() { - m_data = 0x0; - m_cols=0; - m_rows=0; - } - matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + 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<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols)); } ~matxx() { idFree(m_data); } @@ -107,69 +115,86 @@ private: idScalar* m_data; }; -class mat3x { +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) { - bt_id_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 + 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) + { + bt_id_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<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));} - void free() { idFree(m_data);} - enum {m_rows=3}; - int m_cols; - idScalar* m_data; + void allocate() { m_data = static_cast<idScalar*>(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(); +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) { +inline const vec3& vec3::operator=(const vec3& rhs) +{ + if (&rhs != this) + { memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); } return *this; } -inline vec3 vec3::cross(const vec3& b) const { +inline vec3 vec3::cross(const vec3& b) const +{ vec3 result; result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; @@ -178,17 +203,21 @@ inline vec3 vec3::cross(const vec3& b) const { return result; } -inline idScalar vec3::dot(const vec3& b) const { +inline idScalar vec3::dot(const vec3& b) const +{ return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; } -inline const mat33& mat33::operator=(const mat33& rhs) { - if (&rhs != this) { +inline const mat33& mat33::operator=(const mat33& rhs) +{ + if (&rhs != this) + { memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); } return *this; } -inline mat33 mat33::transpose() const { +inline mat33 mat33::transpose() const +{ mat33 result; result.m_data[0] = m_data[0]; result.m_data[1] = m_data[3]; @@ -203,7 +232,8 @@ inline mat33 mat33::transpose() const { return result; } -inline mat33 operator*(const mat33& a, const mat33& b) { +inline mat33 operator*(const mat33& a, const mat33& b) +{ mat33 result; result.m_data[0] = a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; @@ -227,22 +257,27 @@ inline mat33 operator*(const mat33& a, const mat33& b) { return result; } -inline const mat33& mat33::operator+=(const mat33& b) { - for (int i = 0; i < 9; i++) { +inline const mat33& mat33::operator+=(const mat33& b) +{ + for (int i = 0; i < 9; i++) + { m_data[i] += b.m_data[i]; } return *this; } -inline const mat33& mat33::operator-=(const mat33& b) { - for (int i = 0; i < 9; i++) { +inline const mat33& mat33::operator-=(const mat33& b) +{ + for (int i = 0; i < 9; i++) + { m_data[i] -= b.m_data[i]; } return *this; } -inline vec3 operator*(const mat33& a, const vec3& b) { +inline vec3 operator*(const mat33& a, const vec3& b) +{ vec3 result; result.m_data[0] = @@ -255,23 +290,29 @@ inline vec3 operator*(const mat33& a, const vec3& b) { return result; } -inline const vec3& vec3::operator+=(const vec3& b) { - for (int i = 0; i < 3; i++) { +inline const vec3& vec3::operator+=(const vec3& b) +{ + for (int i = 0; i < 3; i++) + { m_data[i] += b.m_data[i]; } return *this; } -inline const vec3& vec3::operator-=(const vec3& b) { - for (int i = 0; i < 3; i++) { +inline const vec3& vec3::operator-=(const vec3& b) +{ + for (int i = 0; i < 3; i++) + { m_data[i] -= b.m_data[i]; } return *this; } -inline mat33 operator*(const mat33& a, const idScalar& s) { +inline mat33 operator*(const mat33& a, const idScalar& s) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] * s; } return result; @@ -279,137 +320,170 @@ inline mat33 operator*(const mat33& a, const idScalar& s) { inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -inline vec3 operator*(const vec3& a, const idScalar& s) { +inline vec3 operator*(const vec3& a, const idScalar& s) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] * s; } return result; } inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; } -inline mat33 operator+(const mat33& a, const mat33& b) { +inline mat33 operator+(const mat33& a, const mat33& b) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] + b.m_data[i]; } return result; } -inline vec3 operator+(const vec3& a, const vec3& b) { +inline vec3 operator+(const vec3& a, const vec3& b) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] + b.m_data[i]; } return result; } -inline mat33 operator-(const mat33& a, const mat33& b) { +inline mat33 operator-(const mat33& a, const mat33& b) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] - b.m_data[i]; } return result; } -inline vec3 operator-(const vec3& a, const vec3& b) { +inline vec3 operator-(const vec3& a, const vec3& b) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] - b.m_data[i]; } return result; } -inline mat33 operator/(const mat33& a, const idScalar& s) { +inline mat33 operator/(const mat33& a, const idScalar& s) +{ mat33 result; - for (int i = 0; i < 9; i++) { + for (int i = 0; i < 9; i++) + { result.m_data[i] = a.m_data[i] / s; } return result; } -inline vec3 operator/(const vec3& a, const idScalar& s) { +inline vec3 operator/(const vec3& a, const idScalar& s) +{ vec3 result; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { result.m_data[i] = a.m_data[i] / s; } return result; } -inline const vecx& vecx::operator=(const vecx& rhs) { - if (size() != rhs.size()) { +inline const vecx& vecx::operator=(const vecx& rhs) +{ + if (size() != rhs.size()) + { bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); abort(); } - if (&rhs != this) { + if (&rhs != this) + { memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); } return *this; } -inline vecx operator*(const vecx& a, const idScalar& s) { +inline vecx operator*(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result.m_data[i] = a.m_data[i] * s; } return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) { +inline vecx operator+(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result.m_data[i] = a.m_data[i] + b.m_data[i]; } return result; } -inline vecx operator-(const vecx& a, const vecx& b) { +inline vecx operator-(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result.m_data[i] = a.m_data[i] - b.m_data[i]; } return result; } -inline vecx operator/(const vecx& a, const idScalar& s) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result.m_data[i] = a.m_data[i] / s; } return result; } -inline vec3 operator*(const mat3x& a, const vecx& b) { - vec3 result; - if (a.cols() != b.size()) { - bt_id_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<b.size();i++) { - for(int k=0;k<3;k++) { - result(k)+=a(k,i)*b(i); - } - } - return result; +inline vec3 operator*(const mat3x& a, const vecx& b) +{ + vec3 result; + if (a.cols() != b.size()) + { + bt_id_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 < b.size(); i++) + { + for (int k = 0; k < 3; k++) + { + result(k) += a(k, i) * b(i); + } + } + return result; } -inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ - (*m)(row, col) = val; +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; +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) +{ + (*m)(row, col) = val; } -} // namespace btInverseDynamcis +} // namespace btInverseDynamics #endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index e8563238c3..befbc2e2a4 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -1,16 +1,16 @@ #include "MultiBodyTreeImpl.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - ,m_m3x(3,m_num_dofs) + , + m_m3x(3, m_num_dofs) #endif { - #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - resize(m_m3x,m_num_dofs); + resize(m_m3x, m_num_dofs); #endif m_body_list.resize(num_bodies_); m_parent_index.resize(num_bodies_); @@ -23,8 +23,10 @@ MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) m_world_gravity(2) = -9.8; } -const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { - switch (type) { +const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const +{ + switch (type) + { case FIXED: return "fixed"; case REVOLUTE: @@ -33,22 +35,28 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ return "prismatic"; case FLOATING: return "floating"; + case SPHERICAL: + return "spherical"; } return "error: invalid"; } -inline void indent(const int &level) { +inline void indent(const int &level) +{ for (int j = 0; j < level; j++) id_printf(" "); // indent } -void MultiBodyTree::MultiBodyImpl::printTree() { +void MultiBodyTree::MultiBodyImpl::printTree() +{ id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); printTree(0, 0); } -void MultiBodyTree::MultiBodyImpl::printTreeData() { - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { +void MultiBodyTree::MultiBodyImpl::printTreeData() +{ + for (idArrayIdx i = 0; i < m_body_list.size(); i++) + { RigidBody &body = m_body_list[i]; id_printf("body: %d\n", static_cast<int>(i)); id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); @@ -59,19 +67,22 @@ void MultiBodyTree::MultiBodyImpl::printTreeData() { id_printf("mass = %f\n", body.m_mass); id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), body.m_body_mass_com(2)); - id_printf("I_o= [%f %f %f;\n" - " %f %f %f;\n" - " %f %f %f]\n", - body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), - body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), - body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); + id_printf( + "I_o= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), + body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), + body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); } } -int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { - switch (type) { +int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const +{ + switch (type) + { case FIXED: return 0; case REVOLUTE: @@ -79,12 +90,15 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { return 1; case FLOATING: return 6; + case SPHERICAL: + return 3; } bt_id_error_message("unknown joint type %d\n", type); return 0; } -void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { +void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) +{ // this is adapted from URDF2Bullet. // TODO: fix this and print proper graph (similar to git --log --graph) int num_children = m_child_indices[index].size(); @@ -92,7 +106,8 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { indentation += 2; int count = 0; - for (int i = 0; i < num_children; i++) { + for (int i = 0; i < num_children; i++) + { int child_index = m_child_indices[index][i]; indent(indentation); id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, @@ -104,19 +119,23 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { } } -int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { +int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) +{ m_world_gravity = gravity; return 0; } -int MultiBodyTree::MultiBodyImpl::generateIndexSets() { +int MultiBodyTree::MultiBodyImpl::generateIndexSets() +{ m_body_revolute_list.resize(0); m_body_prismatic_list.resize(0); int q_index = 0; - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_list.size(); i++) + { RigidBody &body = m_body_list[i]; body.m_q_index = -1; - switch (body.m_joint_type) { + switch (body.m_joint_type) + { case REVOLUTE: m_body_revolute_list.push_back(i); body.m_q_index = q_index; @@ -135,28 +154,41 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { body.m_q_index = q_index; q_index += 6; break; + case SPHERICAL: + m_body_spherical_list.push_back(i); + body.m_q_index = q_index; + q_index += 3; + break; default: bt_id_error_message("unsupported joint type %d\n", body.m_joint_type); return -1; } } // sanity check - if (q_index != m_num_dofs) { + if (q_index != m_num_dofs) + { bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); return -1; } m_child_indices.resize(m_body_list.size()); - for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { + for (idArrayIdx child = 1; child < m_parent_index.size(); child++) + { const int &parent = m_parent_index[child]; - if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) { + if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) + { m_child_indices[parent].push_back(child); - } else { - if (-1 == parent) { + } + else + { + if (-1 == parent) + { // multiple bodies are directly linked to the environment, ie, not a single root bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); - } else { + } + else + { // should never happen bt_id_error_message( "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", @@ -169,11 +201,14 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { return 0; } -void MultiBodyTree::MultiBodyImpl::calculateStaticData() { +void MultiBodyTree::MultiBodyImpl::calculateStaticData() +{ // relative kinematics that are not a function of q, u, dot_u - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_list.size(); i++) + { RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) { + switch (body.m_joint_type) + { case REVOLUTE: body.m_parent_vel_rel(0) = 0; body.m_parent_vel_rel(1) = 0; @@ -212,41 +247,56 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() { case FLOATING: // no static data break; + case SPHERICAL: + //todo: review + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + break; } - // resize & initialize jacobians to zero. + // resize & initialize jacobians to zero. #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - body.m_body_dot_Jac_T_u(0) = 0.0; - body.m_body_dot_Jac_T_u(1) = 0.0; - body.m_body_dot_Jac_T_u(2) = 0.0; - body.m_body_dot_Jac_R_u(0) = 0.0; - body.m_body_dot_Jac_R_u(1) = 0.0; - body.m_body_dot_Jac_R_u(2) = 0.0; - resize(body.m_body_Jac_T,m_num_dofs); - resize(body.m_body_Jac_R,m_num_dofs); - body.m_body_Jac_T.setZero(); - body.m_body_Jac_R.setZero(); -#endif // + body.m_body_dot_Jac_T_u(0) = 0.0; + body.m_body_dot_Jac_T_u(1) = 0.0; + body.m_body_dot_Jac_T_u(2) = 0.0; + body.m_body_dot_Jac_R_u(0) = 0.0; + body.m_body_dot_Jac_R_u(1) = 0.0; + body.m_body_dot_Jac_R_u(2) = 0.0; + resize(body.m_body_Jac_T, m_num_dofs); + resize(body.m_body_Jac_R, m_num_dofs); + body.m_body_Jac_T.setZero(); + body.m_body_Jac_R.setZero(); +#endif // } } int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, - const vecx &dot_u, vecx *joint_forces) { + const vecx &dot_u, vecx *joint_forces) +{ if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || - joint_forces->size() != m_num_dofs) { - bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), - static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size())); + joint_forces->size() != m_num_dofs) + { + bt_id_error_message( + "wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), + static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size())); return -1; } // 1. relative kinematics - if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { - bt_id_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++) { + if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION)) + { + bt_id_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) body.m_eom_lhs_rotational = @@ -268,14 +318,16 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const // Also, this enables adding zero weight bodies as a way to calculate frame poses // for force elements, etc. - for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { + for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) + { // sum of forces and moments acting on this body from its children vec3 sum_f_children; vec3 sum_m_children; setZero(sum_f_children); setZero(sum_m_children); for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); - child_list_idx++) { + child_list_idx++) + { const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; vec3 child_joint_force_in_this_frame = child.m_body_T_parent.transpose() * child.m_force_at_joint; @@ -293,19 +345,22 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const // These are the components of force_at_joint/moment_at_joint // in the free directions given by Jac_JT/Jac_JR // 4.1 revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) + { RigidBody &body = m_body_list[m_body_revolute_list[i]]; // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); } // 4.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) + { RigidBody &body = m_body_list[m_body_prismatic_list[i]]; // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); } // 4.3 floating bodies (6-DoF joints) - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) + { RigidBody &body = m_body_list[m_body_floating_list[i]]; (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); @@ -316,84 +371,133 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); } + // 4.4 spherical bodies (3-DoF joints) + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_list[i]]; + (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); + (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); + (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); + } 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 ) { - bt_id_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<int>(q.size()), static_cast<int>(u.size()), - static_cast<int>(dot_u.size())); +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) + { + bt_id_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<int>(q.size()), static_cast<int>(u.size()), + static_cast<int>(dot_u.size())); + return -1; + } + if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) + { + bt_id_error_message("invalid type %d\n", type); return -1; } - if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { - bt_id_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++) { + 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); - } + 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++) { + 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); - } + 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++) { + 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)); + 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); + 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(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_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(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; - } + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } + } + + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_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 = 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 = 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 = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } } // 2. absolute kinematic quantities (vector valued) @@ -410,26 +514,29 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx 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; - } + 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++) { + 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: @@ -439,121 +546,159 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx 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); - } + 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; + 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; - } +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; + case SPHERICAL: + //todo: review + 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); + 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) { - bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); - return -1; - } - if(type != POSITION_ONLY && type != POSITION_VELOCITY) { - bt_id_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; +int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type) +{ + if (q.size() != m_num_dofs || u.size() != m_num_dofs) + { + bt_id_error_message( + "wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); + return -1; + } + if (type != POSITION_ONLY && type != POSITION_VELOCITY) + { + bt_id_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) { +static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) +{ + switch (dof) + { + // rotational part + case 0: + Jac_JR(0) = 1; + Jac_JR(1) = 0; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 1: + Jac_JR(0) = 0; + Jac_JR(1) = 1; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 2: + Jac_JR(0) = 0; + Jac_JR(1) = 0; + Jac_JR(2) = 1; + setZero(Jac_JT); + break; + } +} + +static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) +{ + switch (dof) + { // rotational part case 0: Jac_JR(0) = 1; @@ -595,8 +740,10 @@ static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) } } -static inline int jointNumDoFs(const JointType &type) { - switch (type) { +static inline int jointNumDoFs(const JointType &type) +{ + switch (type) + { case FIXED: return 0; case REVOLUTE: @@ -604,6 +751,8 @@ static inline int jointNumDoFs(const JointType &type) { return 1; case FLOATING: return 6; + case SPHERICAL: + return 3; } // this should never happen bt_id_error_message("invalid joint type\n"); @@ -615,37 +764,45 @@ static inline int jointNumDoFs(const JointType &type) { int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx *mass_matrix) { -// This calculates the joint space mass matrix for the multibody system. -// The algorithm is essentially an implementation of "method 3" -// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) -// (Later named "Composite Rigid Body Algorithm" by Featherstone). -// -// 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. + matxx *mass_matrix) +{ + // This calculates the joint space mass matrix for the multibody system. + // The algorithm is essentially an implementation of "method 3" + // in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) + // (Later named "Composite Rigid Body Algorithm" by Featherstone). + // + // 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. if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || - mass_matrix->cols() != m_num_dofs) { - bt_id_error_message("Dimension error. System has %d DOFs,\n" - "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", - m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()), - static_cast<int>(mass_matrix->cols())); + mass_matrix->cols() != m_num_dofs) + { + bt_id_error_message( + "Dimension error. System has %d DOFs,\n" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()), + static_cast<int>(mass_matrix->cols())); return -1; } // TODO add optimized zeroing function? - if (initialize_matrix) { - for (int i = 0; i < m_num_dofs; i++) { - for (int j = 0; j < m_num_dofs; j++) { - setMatxxElem(i, j, 0.0, mass_matrix); + if (initialize_matrix) + { + for (int i = 0; i < m_num_dofs; i++) + { + for (int j = 0; j < m_num_dofs; j++) + { + setMatxxElem(i, j, 0.0, mass_matrix); } } } - if (update_kinematics) { + if (update_kinematics) + { // 1. update relative kinematics // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) + { RigidBody &body = m_body_list[m_body_revolute_list[i]]; // from reference orientation (q=0) of body-fixed frame to current orientation mat33 body_T_body_ref; @@ -653,7 +810,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; } // 1.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + 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 = @@ -661,7 +819,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } // 1.3 fixed joints: nothing to do // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + 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)) * @@ -674,7 +833,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; } } - for (int i = m_body_list.size() - 1; i >= 0; i--) { + for (int i = m_body_list.size() - 1; i >= 0; i--) + { RigidBody &body = m_body_list[i]; // calculate mass, center of mass and inertia of "composite rigid body", // ie, sub-tree starting at current body @@ -682,7 +842,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_body_subtree_mass_com = body.m_body_mass_com; body.m_body_subtree_I_body = body.m_body_I_body; - for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { + for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) + { RigidBody &child = m_body_list[m_child_indices[i][c]]; mat33 body_T_child = child.m_body_T_parent.transpose(); @@ -692,7 +853,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_body_subtree_I_body += body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; - if (child.m_subtree_mass > 0) { + if (child.m_subtree_mass > 0) + { // Shift the reference point for the child subtree inertia using the // Huygens-Steiner ("parallel axis") theorem. // (First shift from child origin to child com, then from there to this body's @@ -707,7 +869,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } } - for (int i = m_body_list.size() - 1; i >= 0; i--) { + for (int i = m_body_list.size() - 1; i >= 0; i--) + { const RigidBody &body = m_body_list[i]; // determine DoF-range for body @@ -717,11 +880,18 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // local joint jacobians (ok as is for 1-DoF joints) vec3 Jac_JR = body.m_Jac_JR; vec3 Jac_JT = body.m_Jac_JT; - for (int col = q_index_max; col >= q_index_min; col--) { + for (int col = q_index_max; col >= q_index_min; col--) + { // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) { + if (FLOATING == body.m_joint_type) + { setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); } + if (SPHERICAL == body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); + } vec3 body_eom_rot = body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); @@ -732,19 +902,27 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // rest of the mass matrix column upwards { // 1. for multi-dof joints, rest of the dofs of this body - for (int row = col - 1; row >= q_index_min; row--) { - if (FLOATING != body.m_joint_type) { - bt_id_error_message("??\n"); - return -1; + for (int row = col - 1; row >= q_index_min; row--) + { + if (SPHERICAL == body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMatxxElem(col, row, Mrc, mass_matrix); + } + if (FLOATING == body.m_joint_type) + { + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMatxxElem(col, row, Mrc, mass_matrix); } - setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMatxxElem(col, row, Mrc, mass_matrix); } // 2. ancestor dofs int child_idx = i; int parent_idx = m_parent_index[i]; - while (parent_idx >= 0) { + while (parent_idx >= 0) + { const RigidBody &child_body = m_body_list[child_idx]; const RigidBody &parent_body = m_body_list[parent_idx]; @@ -758,9 +936,16 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; vec3 Jac_JR = parent_body.m_Jac_JR; vec3 Jac_JT = parent_body.m_Jac_JT; - for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) + { + if (SPHERICAL == parent_body.m_joint_type) + { + //todo: review + setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } // set jacobians for 6-DoF joints - if (FLOATING == parent_body.m_joint_type) { + if (FLOATING == parent_body.m_joint_type) + { 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); @@ -774,10 +959,13 @@ 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++) { - setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); + if (set_lower_triangular_matrix) + { + for (int col = 0; col < m_num_dofs; col++) + { + for (int row = 0; row < col; row++) + { + setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); } } } @@ -785,76 +973,91 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool } // utility macro -#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ - do { \ - if (index < 0 || index >= m_num_bodies) { \ - bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ - return -1; \ - } \ +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do \ + { \ + if (index < 0 || index >= m_num_bodies) \ + { \ + bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ + return -1; \ + } \ } while (0) -int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) { +int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *p = m_parent_index[body_index]; return 0; } -int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { +int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *user_int = m_user_int[body_index]; return 0; } -int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { +int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *user_ptr = m_user_ptr[body_index]; return 0; } -int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { +int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_user_int[body_index] = user_int; return 0; } -int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { +int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_user_ptr[body_index] = user_ptr; return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { +int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { +int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; - if (body.m_mass > 0) { + if (body.m_mass > 0) + { *world_com = body.m_body_T_world.transpose() * (body.m_body_pos + body.m_body_mass_com / body.m_mass); - } else { + } + else + { *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); } return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const { +int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; *world_T_body = body.m_body_T_world.transpose(); return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { +int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, - vec3 *world_velocity) const { + vec3 *world_velocity) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; @@ -862,13 +1065,17 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const { + vec3 *world_velocity) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; vec3 com; - if (body.m_mass > 0) { + if (body.m_mass > 0) + { com = body.m_body_mass_com / body.m_mass; - } else { + } + else + { com(0) = 0; com(1) = 0; com(2) = 0; @@ -880,149 +1087,173 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, } int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const { + vec3 *world_dot_omega) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, - vec3 *world_acceleration) const { + vec3 *world_acceleration) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; return 0; } -int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const { +int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *joint_type = m_body_list[body_index].m_joint_type; return 0; } int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, - const char **joint_type) const { + const char **joint_type) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); 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::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::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::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 { +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; return 0; } -int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { +int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_mass = mass; return 0; } int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3& first_mass_moment) { + const vec3 &first_mass_moment) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_body_mass_com = first_mass_moment; return 0; } int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, - const mat33& second_mass_moment) { + const mat33 &second_mass_moment) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_body_I_body = second_mass_moment; return 0; } -int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const { +int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *mass = m_body_list[body_index].m_mass; return 0; } int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, - vec3 *first_mass_moment) const { + vec3 *first_mass_moment) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *first_mass_moment = m_body_list[body_index].m_body_mass_com; return 0; } int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, - mat33 *second_mass_moment) const { + mat33 *second_mass_moment) const +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); *second_mass_moment = m_body_list[body_index].m_body_I_body; return 0; } -void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() { - for (int index = 0; index < m_num_bodies; index++) { +void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() +{ + for (int index = 0; index < m_num_bodies; index++) + { RigidBody &body = m_body_list[index]; setZero(body.m_body_force_user); setZero(body.m_body_moment_user); } } -int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { +int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_body_force_user += body_force; return 0; } -int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { +int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) +{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_body_moment_user += body_moment; 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::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::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::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; +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 -} +} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 3efe9d0492..eabdbe161b 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -8,12 +8,13 @@ #include "../IDConfig.hpp" #include "../MultiBodyTree.hpp" -namespace btInverseDynamics { - +namespace btInverseDynamics +{ /// Structure for for rigid body mass properties, connectivity and kinematic state /// all vectors and matrices are in body-fixed frame, if not indicated otherwise. /// The body-fixed frame is located in the joint connecting the body to its parent. -struct RigidBody { +struct RigidBody +{ ID_DECLARE_ALIGNED_ALLOCATOR(); // 1 Inertial properties /// Mass @@ -112,31 +113,33 @@ struct RigidBody { 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; + /// 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 -class MultiBodyTree::MultiBodyImpl { +class MultiBodyTree::MultiBodyImpl +{ friend class MultiBodyTree; public: ID_DECLARE_ALIGNED_ALLOCATOR(); - enum KinUpdateType { - POSITION_ONLY, - POSITION_VELOCITY, - POSITION_VELOCITY_ACCELERATION - }; + enum KinUpdateType + { + POSITION_ONLY, + POSITION_VELOCITY, + POSITION_VELOCITY_ACCELERATION + }; /// constructor /// @param num_bodies the number of bodies in the system @@ -150,24 +153,24 @@ 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); + /// 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); + /// 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 @@ -190,12 +193,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::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 @@ -271,13 +274,15 @@ private: idArray<int>::type m_body_prismatic_list; // Indices of floating joints idArray<int>::type m_body_floating_list; + // Indices of spherical joints + idArray<int>::type m_body_spherical_list; // a user-provided integer idArray<int>::type m_user_int; // a user-provided pointer idArray<void*>::type m_user_ptr; #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - mat3x m_m3x; + mat3x m_m3x; #endif }; -} +} // namespace btInverseDynamics #endif diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index e9511b7076..a718db051e 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -1,12 +1,13 @@ #include "MultiBodyTreeInitCache.hpp" -namespace btInverseDynamics { - -MultiBodyTree::InitCache::InitCache() { +namespace btInverseDynamics +{ +MultiBodyTree::InitCache::InitCache() +{ m_inertias.resize(0); m_joints.resize(0); m_num_dofs = 0; - m_root_index=-1; + m_root_index = -1; } int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, @@ -15,8 +16,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind const mat33& body_T_parent_ref, const vec3& body_axis_of_motion, const idScalar mass, const vec3& body_r_body_com, const mat33& body_I_body, - const int user_int, void* user_ptr) { - switch (joint_type) { + const int user_int, void* user_ptr) +{ + switch (joint_type) + { case REVOLUTE: case PRISMATIC: m_num_dofs += 1; @@ -25,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind // does not add a degree of freedom // m_num_dofs+=0; break; + case SPHERICAL: + m_num_dofs += 3; + break; case FLOATING: m_num_dofs += 6; break; @@ -33,13 +39,15 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind return -1; } - if(-1 == parent_index) { - if(m_root_index>=0) { + if (-1 == parent_index) + { + if (m_root_index >= 0) + { bt_id_error_message("trying to add body %d as root, but already added %d as root body\n", - body_index, m_root_index); + body_index, m_root_index); return -1; } - m_root_index=body_index; + m_root_index = body_index; } JointData joint; @@ -61,8 +69,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind m_user_ptr.push_back(user_ptr); return 0; } -int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { - if (index < 0 || index > static_cast<int>(m_inertias.size())) { +int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const +{ + if (index < 0 || index > static_cast<int>(m_inertias.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -71,8 +81,10 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert return 0; } -int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { - if (index < 0 || index > static_cast<int>(m_user_int.size())) { +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const +{ + if (index < 0 || index > static_cast<int>(m_user_int.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -80,8 +92,10 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { return 0; } -int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { - if (index < 0 || index > static_cast<int>(m_user_ptr.size())) { +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const +{ + if (index < 0 || index > static_cast<int>(m_user_ptr.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -89,8 +103,10 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const return 0; } -int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { - if (index < 0 || index > static_cast<int>(m_joints.size())) { +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const +{ + if (index < 0 || index > static_cast<int>(m_joints.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -98,16 +114,18 @@ int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) co return 0; } -int MultiBodyTree::InitCache::buildIndexSets() { +int MultiBodyTree::InitCache::buildIndexSets() +{ // NOTE: This function assumes that proper indices were provided // User2InternalIndex from utils can be used to facilitate this. m_parent_index.resize(numBodies()); - for (idArrayIdx j = 0; j < m_joints.size(); j++) { + for (idArrayIdx j = 0; j < m_joints.size(); j++) + { const JointData& joint = m_joints[j]; m_parent_index[joint.m_child] = joint.m_parent; } return 0; } -} +} // namespace btInverseDynamics diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp index 0d2aa4a071..dbdb3ff604 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -5,9 +5,11 @@ #include "../IDMath.hpp" #include "../MultiBodyTree.hpp" -namespace btInverseDynamics { +namespace btInverseDynamics +{ /// Mass properties of a rigid body -struct InertiaData { +struct InertiaData +{ ID_DECLARE_ALIGNED_ALLOCATOR(); /// mass @@ -21,7 +23,8 @@ struct InertiaData { }; /// Joint properties -struct JointData { +struct JointData +{ ID_DECLARE_ALIGNED_ALLOCATOR(); /// type of joint @@ -48,7 +51,8 @@ struct JointData { /// Data structure to store data passed by the user. /// This is used in MultiBodyTree::finalize to build internal data structures. -class MultiBodyTree::InitCache { +class MultiBodyTree::InitCache +{ public: ID_DECLARE_ALIGNED_ALLOCATOR(); /// constructor @@ -105,5 +109,5 @@ private: // index of root body (or -1 if not set) int m_root_index; }; -} +} // namespace btInverseDynamics #endif // MULTIBODYTREEINITCACHE_HPP_ |