diff --git a/Extras/InverseDynamics/CoilCreator.cpp b/Extras/InverseDynamics/CoilCreator.cpp new file mode 100644 index 000000000..905ba7a57 --- /dev/null +++ b/Extras/InverseDynamics/CoilCreator.cpp @@ -0,0 +1,67 @@ +#include + +#include "CoilCreator.hpp" + +namespace btInverseDynamics { +CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) { + for (int i = 0; i < m_num_bodies; i++) { + m_parent[i] = i - 1; + } + + // DH parameters (that's what's in the paper ...) + const idScalar theta_DH = 0; + const idScalar d_DH = 0.0; + const idScalar a_DH = 1.0 / m_num_bodies; + const idScalar alpha_DH = 5.0 * M_PI / m_num_bodies; + getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref, + &m_body_T_parent_ref); + // always z-axis + m_body_axis_of_motion(0) = 0.0; + m_body_axis_of_motion(1) = 0.0; + m_body_axis_of_motion(2) = 1.0; + + m_mass = 1.0 / m_num_bodies; + m_body_r_body_com(0) = 1.0 / (2.0 * m_num_bodies); + m_body_r_body_com(1) = 0.0; + m_body_r_body_com(2) = 0.0; + + m_body_I_body(0, 0) = 1e-4 / (2.0 * m_num_bodies); + m_body_I_body(0, 1) = 0.0; + m_body_I_body(0, 2) = 0.0; + m_body_I_body(1, 0) = 0.0; + m_body_I_body(1, 1) = (3e-4 + 4.0 / pow(m_num_bodies, 2)) / (12.0 * m_num_bodies); + m_body_I_body(1, 2) = 0.0; + m_body_I_body(2, 0) = 0.0; + m_body_I_body(2, 1) = 0.0; + m_body_I_body(2, 2) = m_body_I_body(1, 1); +} + +CoilCreator::~CoilCreator() {} + +int CoilCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int CoilCreator::getBody(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 (body_index < 0 || body_index >= m_num_bodies) { + error_message("invalid body index %d\n", body_index); + return -1; + } + *parent_index = m_parent[body_index]; + *joint_type = REVOLUTE; + *parent_r_parent_body_ref = m_parent_r_parent_body_ref; + *body_T_parent_ref = m_body_T_parent_ref; + *body_axis_of_motion = m_body_axis_of_motion; + *mass = m_mass; + *body_r_body_com = m_body_r_body_com; + *body_I_body = m_body_I_body; + + *user_int = 0; + *user_ptr = 0; + return 0; +} +} diff --git a/Extras/InverseDynamics/CoilCreator.hpp b/Extras/InverseDynamics/CoilCreator.hpp new file mode 100644 index 000000000..a57ee3595 --- /dev/null +++ b/Extras/InverseDynamics/CoilCreator.hpp @@ -0,0 +1,40 @@ +#ifndef COILCREATOR_HPP_ +#define COILCREATOR_HPP_ + +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +/// Creator class for building a "coil" system as intruduced as benchmark example in +/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n)) +/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International +/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628. +/// +/// This is a serial chain, with an initial configuration resembling a coil. +class CoilCreator : public MultiBodyTreeCreator { +public: + /// ctor. + /// @param n the number of bodies in the system + CoilCreator(int n); + /// dtor + ~CoilCreator(); + // \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; + std::vector m_parent; + vec3 m_parent_r_parent_body_ref; + mat33 m_body_T_parent_ref; + vec3 m_body_axis_of_motion; + idScalar m_mass; + vec3 m_body_r_body_com; + mat33 m_body_I_body; +}; +} +#endif diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp new file mode 100644 index 000000000..cdb3e06a6 --- /dev/null +++ b/Extras/InverseDynamics/DillCreator.cpp @@ -0,0 +1,122 @@ +#include "DillCreator.hpp" +#include +namespace btInverseDynamics { + +DillCreator::DillCreator(int level) + : m_level(level), + m_num_bodies(std::pow(2, level)), + m_parent(m_num_bodies), + m_parent_r_parent_body_ref(m_num_bodies), + m_body_T_parent_ref(m_num_bodies), + m_body_axis_of_motion(m_num_bodies), + m_mass(m_num_bodies), + m_body_r_body_com(m_num_bodies), + m_body_I_body(m_num_bodies) { + // generate names (for debugging) + for (int i = 0; i < m_num_bodies; i++) { + m_parent[i] = i - 1; + + // all z-axis (DH convention) + m_body_axis_of_motion[i](0) = 0.0; + m_body_axis_of_motion[i](1) = 0.0; + m_body_axis_of_motion[i](2) = 1.0; + } + + // recursively build data structures + m_current_body = 0; + const int parent = -1; + const idScalar d_DH = 0.0; + const idScalar a_DH = 0.0; + const idScalar alpha_DH = 0.0; + + if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) { + error_message("recurseDill failed\n"); + abort(); + } +} + +DillCreator::~DillCreator() {} + +int DillCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int DillCreator::getBody(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 (body_index < 0 || body_index >= m_num_bodies) { + error_message("invalid body index %d\n", body_index); + return -1; + } + *parent_index = m_parent[body_index]; + *joint_type = REVOLUTE; + *parent_r_parent_body_ref = m_parent_r_parent_body_ref[body_index]; + *body_T_parent_ref = m_body_T_parent_ref[body_index]; + *body_axis_of_motion = m_body_axis_of_motion[body_index]; + *mass = m_mass[body_index]; + *body_r_body_com = m_body_r_body_com[body_index]; + *body_I_body = m_body_I_body[body_index]; + + *user_int = 0; + *user_ptr = 0; + return 0; +} + +int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in, + const idScalar a_DH_in, const idScalar alpha_DH_in) { + if (level < 0) { + error_message("invalid level parameter (%d)\n", level); + return -1; + } + + if (m_current_body >= m_num_bodies || m_current_body < 0) { + error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body, + m_num_bodies); + return -1; + } + + idScalar size = std::max(level, 1); + const int body = m_current_body; + // length = 0.1 * size; + // with = 2 * 0.01 * size; + + /// these parameters are from the paper ... + /// TODO: add proper citation + m_parent[body] = parent; + m_mass[body] = 0.1 * std::pow(size, 3); + m_body_r_body_com[body](0) = 0.05 * size; + m_body_r_body_com[body](1) = 0; + m_body_r_body_com[body](2) = 0; + // initialization + for (int i = 0; i < 3; i++) { + m_parent_r_parent_body_ref[body](i) = 0; + for (int j = 0; j < 3; j++) { + m_body_I_body[body](i, j) = 0.0; + m_body_T_parent_ref[body](i, j) = 0.0; + } + } + const idScalar size_5 = pow(size, 5); + m_body_I_body[body](0, 0) = size_5 / 0.2e6; + m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6; + m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1); + + getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body], + &m_body_T_parent_ref[body]); + + // attach "level" Dill systems of levels 1...level + for (int i = 1; i <= level; i++) { + idScalar d_DH = 0.01 * size; + if (i == level) { + d_DH = 0.0; + } + const idScalar a_DH = i * 0.1; + const idScalar alpha_DH = i * M_PI / 3.0; + m_current_body++; + recurseDill(i - 1, body, d_DH, a_DH, alpha_DH); + } + + return 0; // ok! +} +} diff --git a/Extras/InverseDynamics/DillCreator.hpp b/Extras/InverseDynamics/DillCreator.hpp new file mode 100644 index 000000000..42f1b1ba0 --- /dev/null +++ b/Extras/InverseDynamics/DillCreator.hpp @@ -0,0 +1,47 @@ +#ifndef DILLCREATOR_HPP_ +#define DILLCREATOR_HPP_ + +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + + +/// Creator class for building a "Dill" system as intruduced as benchmark example in +/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n)) +/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International +/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628. +/// +/// This is a self-similar branched tree, somewhat resembling a dill plant +class DillCreator : public MultiBodyTreeCreator { +public: + /// ctor + /// @param levels the number of dill levels + DillCreator(int levels); + /// dtor + ~DillCreator(); + ///\copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int* num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(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: + /// recursively generate dill bodies. + /// TODO better documentation + int recurseDill(const int levels, const int parent, const idScalar d_DH_in, + const idScalar a_DH_in, const idScalar alpha_DH_in); + int m_level; + int m_num_bodies; + std::vector m_parent; + std::vector m_parent_r_parent_body_ref; + std::vector m_body_T_parent_ref; + std::vector m_body_axis_of_motion; + std::vector m_mass; + std::vector m_body_r_body_com; + std::vector m_body_I_body; + int m_current_body; +}; +} +#endif diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp new file mode 100644 index 000000000..8896eaf55 --- /dev/null +++ b/Extras/InverseDynamics/IDRandomUtil.cpp @@ -0,0 +1,70 @@ +#include +#include +#include + +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/IDMath.hpp" +#include "IDRandomUtil.hpp" + + +namespace btInverseDynamics { + +// constants for random mass and inertia generation +// these are arbitrary positive values. +static const float mass_min = 0.001; +static const float mass_max = 1.0; + +void randomInit() { srand(time(NULL)); } + +int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; } + +float randomFloat(float low, float high) { + return low + static_cast(rand()) / RAND_MAX * (high - low); +} + +float randomMass() { return randomFloat(mass_min, mass_max); } + +vec3 randomInertiaPrincipal() { + vec3 inertia; + do { + inertia(0) = randomFloat(mass_min, mass_max); + inertia(1) = randomFloat(mass_min, mass_max); + inertia(2) = randomFloat(mass_min, mass_max); + } while (inertia(0) + inertia(1) < inertia(2) || inertia(0) + inertia(2) < inertia(1) || + inertia(1) + inertia(2) < inertia(0)); + return inertia; +} + +mat33 randomInertiaMatrix() { + // generate random valid inertia matrix by first getting valid components + // along major axes and then rotating by random amount + vec3 principal = randomInertiaPrincipal(); + mat33 rot(transformX(randomFloat(-M_PI, M_PI)) * transformY(randomFloat(-M_PI, M_PI)) * + transformZ(randomFloat(-M_PI, M_PI))); + mat33 inertia; + inertia(0, 0) = principal(0); + inertia(0, 1) = 0; + inertia(0, 2) = 0; + inertia(1, 0) = 0; + inertia(1, 1) = principal(1); + inertia(1, 2) = 0; + inertia(2, 0) = 0; + inertia(2, 1) = 0; + inertia(2, 2) = principal(2); + return rot * inertia * rot.transpose(); +} + +vec3 randomAxis() { + vec3 axis; + idScalar length; + do { + axis(0) = randomFloat(-1.0, 1.0); + axis(1) = randomFloat(-1.0, 1.0); + axis(2) = randomFloat(-1.0, 1.0); + + length = std::sqrt(std::pow(axis(0), 2) + std::pow(axis(1), 2) + std::pow(axis(2), 2)); + } while (length < 0.01); + + return axis / length; +} +} diff --git a/Extras/InverseDynamics/IDRandomUtil.hpp b/Extras/InverseDynamics/IDRandomUtil.hpp new file mode 100644 index 000000000..a9d363a5b --- /dev/null +++ b/Extras/InverseDynamics/IDRandomUtil.hpp @@ -0,0 +1,34 @@ +#ifndef ID_RANDOM_UTIL_HPP_ +#define ID_RANDOM_UTIL_HPP_ +#include "BulletInverseDynamics/IDConfig.hpp" +namespace btInverseDynamics { +/// seed random number generator +void randomInit(); +/// 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. +/// The results will *not* be perfectly uniform. +/// \param low is the lower bound (inclusive) +/// \param high is the lower bound (inclusive) +/// \return a random number within [\param low, \param high] +int randomInt(int low, int high); +/// Generate a (not quite) uniformly distributed random floats in [low, high] +/// Note: this is a low-quality implementation using only rand(), as +/// C++11 is not supported in bullet. +/// The results will *not* be perfectly uniform. +/// \param low is the lower bound (inclusive) +/// \param high is the lower bound (inclusive) +/// \return a random number within [\param low, \param high] +float randomFloat(float low, float high); + +/// generate a random valid mass value +/// \returns random mass +float randomMass(); +/// generate a random valid vector of principal moments of inertia +vec3 randomInertiaPrincipal(); +/// generate a random valid moment of inertia matrix +mat33 randomInertiaMatrix(); +/// generate a random unit vector +vec3 randomAxis(); +} +#endif diff --git a/Extras/InverseDynamics/MultiBodyNameMap.cpp b/Extras/InverseDynamics/MultiBodyNameMap.cpp new file mode 100644 index 000000000..4e82641c1 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyNameMap.cpp @@ -0,0 +1,78 @@ +#include "MultiBodyNameMap.hpp" + +namespace btInverseDynamics { + +MultiBodyNameMap::MultiBodyNameMap() {} + +int MultiBodyNameMap::addBody(const int index, const std::string& name) { + if (m_index_to_body_name.count(index) > 0) { + error_message("trying to add index %d again\n", index); + return -1; + } + if (m_body_name_to_index.count(name) > 0) { + error_message("trying to add name %s again\n", name.c_str()); + return -1; + } + + m_index_to_body_name[index] = name; + m_body_name_to_index[name] = index; + + return 0; +} + +int MultiBodyNameMap::addJoint(const int index, const std::string& name) { + if (m_index_to_joint_name.count(index) > 0) { + error_message("trying to add index %d again\n", index); + return -1; + } + if (m_joint_name_to_index.count(name) > 0) { + error_message("trying to add name %s again\n", name.c_str()); + return -1; + } + + m_index_to_joint_name[index] = name; + m_joint_name_to_index[name] = index; + + return 0; +} + +int MultiBodyNameMap::getBodyName(const int index, std::string* name) const { + std::map::const_iterator it = m_index_to_body_name.find(index); + if (it == m_index_to_body_name.end()) { + error_message("index %d not known\n", index); + return -1; + } + *name = it->second; + return 0; +} + +int MultiBodyNameMap::getJointName(const int index, std::string* name) const { + std::map::const_iterator it = m_index_to_joint_name.find(index); + if (it == m_index_to_joint_name.end()) { + error_message("index %d not known\n", index); + return -1; + } + *name = it->second; + return 0; +} + +int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const { + std::map::const_iterator it = m_body_name_to_index.find(name); + if (it == m_body_name_to_index.end()) { + error_message("name %s not known\n", name.c_str()); + return -1; + } + *index = it->second; + return 0; +} + +int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const { + std::map::const_iterator it = m_joint_name_to_index.find(name); + if (it == m_joint_name_to_index.end()) { + error_message("name %s not known\n", name.c_str()); + return -1; + } + *index = it->second; + return 0; +} +} diff --git a/Extras/InverseDynamics/MultiBodyNameMap.hpp b/Extras/InverseDynamics/MultiBodyNameMap.hpp new file mode 100644 index 000000000..a5e024ce0 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyNameMap.hpp @@ -0,0 +1,54 @@ +#ifndef MULTIBODYNAMEMAP_HPP_ +#define MULTIBODYNAMEMAP_HPP_ + +#include "BulletInverseDynamics/IDConfig.hpp" +#include +#include + +namespace btInverseDynamics { + +/// \brief The MultiBodyNameMap class +/// Utility class that stores a maps from body/joint indices to/from body and joint names +class MultiBodyNameMap { +public: + MultiBodyNameMap(); + /// add a body to the map + /// @param index of the body + /// @param name name of the body + /// @return 0 on success, -1 on failure + int addBody(const int index, const std::string& name); + /// add a joint to the map + /// @param index of the joint + /// @param name name of the joint + /// @return 0 on success, -1 on failure + int addJoint(const int index, const std::string& name); + /// get body name from index + /// @param index of the body + /// @param body_name name of the body + /// @return 0 on success, -1 on failure + int getBodyName(const int index, std::string* name) const; + /// get joint name from index + /// @param index of the joint + /// @param joint_name name of the joint + /// @return 0 on success, -1 on failure + int getJointName(const int index, std::string* name) const; + /// get body index from name + /// @param index of the body + /// @param name name of the body + /// @return 0 on success, -1 on failure + int getBodyIndex(const std::string& name, int* index) const; + /// get joint index from name + /// @param index of the joint + /// @param name name of the joint + /// @return 0 on success, -1 on failure + int getJointIndex(const std::string& name, int* index) const; + +private: + std::map m_index_to_joint_name; + std::map m_index_to_body_name; + + std::map m_joint_name_to_index; + std::map m_body_name_to_index; +}; +} +#endif // MULTIBODYNAMEMAP_HPP_ diff --git a/Extras/InverseDynamics/MultiBodyTreeCreator.cpp b/Extras/InverseDynamics/MultiBodyTreeCreator.cpp new file mode 100644 index 000000000..300910d7a --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeCreator.cpp @@ -0,0 +1,64 @@ +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) { + int num_bodies; + int parent_index; + JointType joint_type; + vec3 body_r_parent_body_ref; + mat33 body_R_parent_ref; + vec3 body_axis_of_motion; + idScalar mass; + vec3 body_r_body_com; + mat33 body_I_body; + int user_int; + void* user_ptr; + + MultiBodyTree* tree = new MultiBodyTree(); + if (0x0 == tree) { + error_message("cannot allocate tree\n"); + return 0x0; + } + + // TODO: move to some policy argument + tree->setAcceptInvalidMassParameters(false); + + // get number of bodies in the system + if (-1 == creator.getNumBodies(&num_bodies)) { + error_message("getting body indices\n"); + delete tree; + return 0x0; + } + + // get data for all bodies + for (int index = 0; index < num_bodies; index++) { + // get body parameters from user callbacks + if (-1 == + creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref, + &body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com, + &body_I_body, &user_int, &user_ptr)) { + error_message("getting data for body %d\n", index); + delete tree; + return 0x0; + } + // add body to system + if (-1 == + tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref, + body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr)) { + error_message("adding body %d\n", index); + delete tree; + return 0x0; + } + } + // finalize initialization + if (-1 == tree->finalize()) { + error_message("building system\n"); + delete tree; + return 0x0; + } + + return tree; +} +} diff --git a/Extras/InverseDynamics/MultiBodyTreeCreator.hpp b/Extras/InverseDynamics/MultiBodyTreeCreator.hpp new file mode 100644 index 000000000..bbf552eb6 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeCreator.hpp @@ -0,0 +1,45 @@ +#ifndef MULTI_BODY_TREE_CREATOR_HPP_ +#define MULTI_BODY_TREE_CREATOR_HPP_ + +#include +#include +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/IDMath.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "MultiBodyNameMap.hpp" + +namespace btInverseDynamics { +/// Interface class for initializing a MultiBodyTree instance. +/// Data to be provided is modeled on the URDF specification. +/// The user can derive from this class in order to programmatically +/// initialize a system. +class MultiBodyTreeCreator { +public: + /// the dtor + virtual ~MultiBodyTreeCreator() {} + /// Get the number of bodies in the system + /// @param num_bodies write number of bodies here + /// @return 0 on success, -1 on error + virtual int getNumBodies(int* num_bodies) const = 0; + /// Interface for accessing link mass properties. + /// For detailed description of data, @sa MultiBodyTree::addBody + /// \copydoc MultiBodyTree::addBody + virtual 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 = 0; + /// @return a pointer to a name mapping utility class, or 0x0 if not available + virtual const MultiBodyNameMap* getNameMap() const {return 0x0;} +}; + +/// Create a multibody object. +/// @param creator an object implementing the MultiBodyTreeCreator interface +/// that returns data defining the system +/// @return A pointer to an allocated multibodytree instance, or +/// 0x0 if an error occured. +MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator); +} + +// does urdf have gravity direction ?? + +#endif // MULTI_BODY_TREE_CREATOR_HPP_ diff --git a/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp new file mode 100644 index 000000000..587a02be1 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp @@ -0,0 +1,64 @@ +#include "MultiBodyTreeDebugGraph.hpp" + +#include + +namespace btInverseDynamics { + +int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map, + const char* filename) { + if (0x0 == tree) { + error_message("tree pointer is null\n"); + return -1; + } + if (0x0 == filename) { + error_message("filename is null\n"); + return -1; + } + + FILE* fp = fopen(filename, "w"); + if (NULL == fp) { + error_message("cannot open file %s for writing\n", filename); + return -1; + } + fprintf(fp, "// to generate postscript file, run dot -Tps %s -o %s.ps\n" + "// details see graphviz documentation at http://graphviz.org\n" + "digraph tree {\n", + filename, filename); + + for (int body = 0; body < tree->numBodies(); body++) { + std::string name; + if (0x0 != map) { + if (-1 == map->getBodyName(body, &name)) { + error_message("can't get name of body %d\n", body); + return -1; + } + fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str()); + } + } + for (int body = 0; body < tree->numBodies(); body++) { + int parent; + const char* joint_type; + int qi; + if (-1 == tree->getParentIndex(body, &parent)) { + error_message("indexing error\n"); + return -1; + } + if (-1 == tree->getJointTypeStr(body, &joint_type)) { + error_message("indexing error\n"); + return -1; + } + if (-1 == tree->getDoFOffset(body, &qi)) { + error_message("indexing error\n"); + return -1; + } + if (-1 != parent) { + fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body, + joint_type, qi); + } + } + + fprintf(fp, "}\n"); + fclose(fp); + return 0; +} +} diff --git a/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp new file mode 100644 index 000000000..f249d0952 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp @@ -0,0 +1,17 @@ +#ifndef MULTIBODYTREEDEBUGGRAPH_HPP_ +#define MULTIBODYTREEDEBUGGRAPH_HPP_ +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "MultiBodyNameMap.hpp" + +namespace btInverseDynamics { +/// generate a dot-file of the multibody tree for generating a graph using graphviz' dot tool +/// @param tree the multibody tree +/// @param map to add names of links (if 0x0, no names will be added) +/// @param filename name for the output file +/// @return 0 on success, -1 on error +int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map, + const char* filename); +} + +#endif // MULTIBODYTREEDEBUGGRAPH_HPP diff --git a/Extras/InverseDynamics/SimpleTreeCreator.cpp b/Extras/InverseDynamics/SimpleTreeCreator.cpp new file mode 100644 index 000000000..e2b308ebe --- /dev/null +++ b/Extras/InverseDynamics/SimpleTreeCreator.cpp @@ -0,0 +1,69 @@ +#include "SimpleTreeCreator.hpp" + +#include + +namespace btInverseDynamics { +/// minimal "tree" (chain) +SimpleTreeCreator::SimpleTreeCreator(int dim) : m_num_bodies(dim) { + m_mass = 1.0; + m_body_T_parent_ref(0, 0) = 1; + m_body_T_parent_ref(0, 1) = 0; + m_body_T_parent_ref(0, 2) = 0; + m_body_T_parent_ref(1, 0) = 0; + m_body_T_parent_ref(1, 1) = 1; + m_body_T_parent_ref(1, 2) = 0; + m_body_T_parent_ref(2, 0) = 0; + m_body_T_parent_ref(2, 1) = 0; + m_body_T_parent_ref(2, 2) = 1; + + m_parent_r_parent_body_ref(0) = 1.0; + m_parent_r_parent_body_ref(1) = 0.0; + m_parent_r_parent_body_ref(2) = 0.0; + + m_body_r_body_com(0) = 0.5; + m_body_r_body_com(1) = 0.0; + m_body_r_body_com(2) = 0.0; + + m_body_I_body(0, 0) = 1; + m_body_I_body(0, 1) = 0; + m_body_I_body(0, 2) = 0; + m_body_I_body(1, 0) = 0; + m_body_I_body(1, 1) = 1; + m_body_I_body(1, 2) = 0; + m_body_I_body(2, 0) = 0; + m_body_I_body(2, 1) = 0; + m_body_I_body(2, 2) = 1; + + m_axis(0) = 0; + m_axis(1) = 0; + m_axis(2) = 1; +} +int SimpleTreeCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int SimpleTreeCreator::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 { + *parent_index = body_index - 1; + if (body_index % 2) { + *joint_type = PRISMATIC; + } else { + *joint_type = REVOLUTE; + } + *parent_r_parent_body_ref = m_parent_r_parent_body_ref; + if (0 == body_index) { + (*parent_r_parent_body_ref)(2) = 1.0; + } + *body_T_parent_ref = m_body_T_parent_ref; + *body_axis_of_motion = m_axis; + *mass = m_mass; + *body_r_body_com = m_body_r_body_com; + *body_I_body = m_body_I_body; + *user_int = 0; + *user_ptr = 0; + return 0; +} +} diff --git a/Extras/InverseDynamics/SimpleTreeCreator.hpp b/Extras/InverseDynamics/SimpleTreeCreator.hpp new file mode 100644 index 000000000..f336eae72 --- /dev/null +++ b/Extras/InverseDynamics/SimpleTreeCreator.hpp @@ -0,0 +1,34 @@ +#ifndef SIMPLETREECREATOR_HPP_ +#define SIMPLETREECREATOR_HPP_ + +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +/// minimal "tree" (chain) +class SimpleTreeCreator : public MultiBodyTreeCreator { +public: + /// ctor + /// @param dim number of bodies + SimpleTreeCreator(int dim); + // dtor + ~SimpleTreeCreator() {} + ///\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; + idScalar m_mass; + mat33 m_body_T_parent_ref; + vec3 m_parent_r_parent_body_ref; + vec3 m_body_r_body_com; + mat33 m_body_I_body; + vec3 m_axis; +}; +} +#endif // SIMPLETREECREATOR_HPP_ diff --git a/Extras/InverseDynamics/User2InternalIndex.cpp b/Extras/InverseDynamics/User2InternalIndex.cpp new file mode 100644 index 000000000..3e39cb227 --- /dev/null +++ b/Extras/InverseDynamics/User2InternalIndex.cpp @@ -0,0 +1,99 @@ +#include "User2InternalIndex.hpp" + +namespace btInverseDynamics { +User2InternalIndex::User2InternalIndex() : m_map_built(false) {} + +void User2InternalIndex::addBody(const int body, const int parent) { + m_user_parent_index_map[body] = parent; +} + +int User2InternalIndex::findRoot(int index) { + if (0 == m_user_parent_index_map.count(index)) { + return index; + } + return findRoot(m_user_parent_index_map[index]); +} + +// modelled after URDF2Bullet.cpp:void ComputeParentIndices(const +// URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, +// int urdfParentIndex) +void User2InternalIndex::recurseIndexSets(const int user_body_index) { + m_user_to_internal[user_body_index] = m_current_index; + m_current_index++; + for (size_t i = 0; i < m_user_child_indices[user_body_index].size(); i++) { + recurseIndexSets(m_user_child_indices[user_body_index][i]); + } +} + +int User2InternalIndex::buildMapping() { + // find root index + int user_root_index = -1; + for (std::map::iterator it = m_user_parent_index_map.begin(); + it != m_user_parent_index_map.end(); it++) { + int current_root_index = findRoot(it->second); + if (it == m_user_parent_index_map.begin()) { + user_root_index = current_root_index; + } else { + if (user_root_index != current_root_index) { + error_message("multiple roots (at least) %d and %d\n", user_root_index, + current_root_index); + return -1; + } + } + } + + // build child index map + for (std::map::iterator it = m_user_parent_index_map.begin(); + it != m_user_parent_index_map.end(); it++) { + m_user_child_indices[it->second].push_back(it->first); + } + + m_current_index = -1; + // build internal index set + m_user_to_internal[user_root_index] = -1; // add map for root link + recurseIndexSets(user_root_index); + + // reverse mapping + for (std::map::iterator it = m_user_to_internal.begin(); + it != m_user_to_internal.end(); it++) { + m_internal_to_user[it->second] = it->first; + } + + m_map_built = true; + return 0; +} + +int User2InternalIndex::user2internal(const int user, int *internal) const { + + if (!m_map_built) { + return -1; + } + + std::map::const_iterator it; + it = m_user_to_internal.find(user); + if (it != m_user_to_internal.end()) { + *internal = it->second; + return 0; + } else { + error_message("no user index %d\n", user); + return -1; + } +} + +int User2InternalIndex::internal2user(const int internal, int *user) const { + + if (!m_map_built) { + return -1; + } + + std::map::const_iterator it; + it = m_internal_to_user.find(internal); + if (it != m_internal_to_user.end()) { + *user = it->second; + return 0; + } else { + error_message("no internal index %d\n", internal); + return -1; + } +} +} diff --git a/Extras/InverseDynamics/User2InternalIndex.hpp b/Extras/InverseDynamics/User2InternalIndex.hpp new file mode 100644 index 000000000..06c7ef2aa --- /dev/null +++ b/Extras/InverseDynamics/User2InternalIndex.hpp @@ -0,0 +1,46 @@ +#ifndef USER2INTERNALINDEX_HPP +#define USER2INTERNALINDEX_HPP +#include +#include + +#include "BulletInverseDynamics/IDConfig.hpp" + +namespace btInverseDynamics { + +/// Convert arbitrary indexing scheme to internal indexing +/// used for MultiBodyTree +class User2InternalIndex { +public: + /// Ctor + User2InternalIndex(); + /// add body to index maps + /// @param body index of body to add (external) + /// @param parent index of parent body (external) + void addBody(const int body, const int parent); + /// build mapping from external to internal indexing + /// @return 0 on success, -1 on failure + int buildMapping(); + /// get internal index from external index + /// @param user external (user) index + /// @param internal pointer for storage of corresponding internal index + /// @return 0 on success, -1 on failure + int user2internal(const int user, int *internal) const; + /// get internal index from external index + /// @param user external (user) index + /// @param internal pointer for storage of corresponding internal index + /// @return 0 on success, -1 on failure + int internal2user(const int internal, int *user) const; + +private: + int findRoot(int index); + void recurseIndexSets(const int user_body_index); + bool m_map_built; + std::map m_user_parent_index_map; + std::map m_user_to_internal; + std::map m_internal_to_user; + std::map > m_user_child_indices; + int m_current_index; +}; +} + +#endif // USER2INTERNALINDEX_HPP diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp new file mode 100644 index 000000000..978bad743 --- /dev/null +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -0,0 +1,91 @@ +#ifndef BTMULTIBODYFROMURDF_HPP +#define BTMULTIBODYFROMURDF_HPP + +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#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" + +/// Create a btMultiBody model from URDF. +/// This is adapted from Bullet URDF loader example +class MyBtMultiBodyFromURDF { +public: + /// ctor + /// @param gravity gravitational acceleration (in world frame) + /// @param base_fixed if true, the root body is treated as fixed, + /// if false, it is treated as floating + MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed) + : m_gravity(gravity), m_base_fixed(base_fixed) { + m_broadphase = 0x0; + m_dispatcher = 0x0; + m_solver = 0x0; + m_collisionConfiguration = 0x0; + m_dynamicsWorld = 0x0; + m_multibody = 0x0; + } + /// dtor + ~MyBtMultiBodyFromURDF() { + delete m_dynamicsWorld; + delete m_solver; + delete m_broadphase; + delete m_dispatcher; + delete m_collisionConfiguration; + delete m_multibody; + } + /// @param name path to urdf file + void setFileName(const std::string name) { m_filename = name; } + /// load urdf file and build btMultiBody model + void init() { + this->createEmptyDynamicsWorld(); + m_dynamicsWorld->setGravity(m_gravity); + BulletURDFImporter urdf_importer(&m_nogfx); + URDFImporterInterface &u2b(urdf_importer); + bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); + + if (loadOk) { + btTransform identityTrans; + identityTrans.setIdentity(); + MyMultiBodyCreator creation(&m_nogfx); + const bool use_multibody = true; + ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody, + u2b.getPathPrefix()); + m_multibody = creation.getBulletMultiBody(); + m_dynamicsWorld->stepSimulation(1. / 240., 0); + } + } + /// @return pointer to the btMultiBody model + btMultiBody *getBtMultiBody() { return m_multibody; } + +private: + // internal utility function + void createEmptyDynamicsWorld() { + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + /// use the default collision dispatcher. For parallel processing you can use a diffent + /// dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_broadphase = new btDbvtBroadphase(); + m_solver = new btMultiBodyConstraintSolver; + m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, + m_collisionConfiguration); + m_dynamicsWorld->setGravity(m_gravity); + } + + btBroadphaseInterface *m_broadphase; + btCollisionDispatcher *m_dispatcher; + btMultiBodyConstraintSolver *m_solver; + btDefaultCollisionConfiguration *m_collisionConfiguration; + btMultiBodyDynamicsWorld *m_dynamicsWorld; + std::string m_filename; + DummyGUIHelper m_nogfx; + btMultiBody *m_multibody; + const btVector3 m_gravity; + const bool m_base_fixed; +}; +#endif // BTMULTIBODYFROMURDF_HPP diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp new file mode 100644 index 000000000..b91a10cce --- /dev/null +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -0,0 +1,270 @@ +#include "btMultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +btMultiBodyTreeCreator::btMultiBodyTreeCreator() : m_initialized(false) {} + +int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) { + if (0x0 == btmb) { + error_message("cannot create MultiBodyTree from null pointer\n"); + return -1; + } + + // in case this is a second call, discard old data + m_data.clear(); + m_initialized = false; + + // btMultiBody treats base link separately + m_data.resize(1 + btmb->getNumLinks()); + + // add base link data + { + LinkData &link = m_data[0]; + + link.parent_index = -1; + if (btmb->hasFixedBase()) { + link.joint_type = FIXED; + } else { + link.joint_type = FLOATING; + } + btTransform transform(btmb->getBaseWorldTransform()); + + link.parent_r_parent_body_ref(0) = transform.getOrigin()[0]; + link.parent_r_parent_body_ref(1) = transform.getOrigin()[1]; + link.parent_r_parent_body_ref(2) = transform.getOrigin()[2]; + + link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0]; + link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1]; + link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2]; + + link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0]; + link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1]; + link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2]; + + link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0]; + link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1]; + link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2]; + + // random unit vector. value not used for fixed or floating joints. + link.body_axis_of_motion(0) = 0; + link.body_axis_of_motion(1) = 0; + link.body_axis_of_motion(2) = 1; + + link.mass = btmb->getBaseMass(); + // link frame in the center of mass + link.body_r_body_com(0) = 0; + link.body_r_body_com(1) = 0; + link.body_r_body_com(2) = 0; + // BulletDynamics uses body-fixed frame in the cog, aligned with principal axes + link.body_I_body(0, 0) = btmb->getBaseInertia()[0]; + link.body_I_body(0, 1) = 0.0; + link.body_I_body(0, 2) = 0.0; + link.body_I_body(1, 0) = 0.0; + link.body_I_body(1, 1) = btmb->getBaseInertia()[1]; + link.body_I_body(1, 2) = 0.0; + link.body_I_body(2, 0) = 0.0; + link.body_I_body(2, 1) = 0.0; + link.body_I_body(2, 2) = btmb->getBaseInertia()[2]; + // shift reference point to link origin (in joint axis) + mat33 tilde_r_com = tildeOperator(link.body_r_body_com); + link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com; + if (verbose) { + id_printf("base: mass= %f, bt_inertia= [%f %f %f]\n" + "Io= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1], + btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1), + link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1), + link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1), + link.body_I_body(2, 2)); + } + } + + for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++) { + if (verbose) { + id_printf("bt->id: converting link %d\n", bt_index); + } + const btMultibodyLink &bt_link = btmb->getLink(bt_index); + LinkData &link = m_data[bt_index + 1]; + + link.parent_index = bt_link.m_parent + 1; + + link.mass = bt_link.m_mass; + if (verbose) { + id_printf("mass= %f\n", link.mass); + } + // from this body's pivot to this body's com in this body's frame + link.body_r_body_com[0] = bt_link.m_dVector[0]; + link.body_r_body_com[1] = bt_link.m_dVector[1]; + link.body_r_body_com[2] = bt_link.m_dVector[2]; + if (verbose) { + id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1], + link.body_r_body_com[2]); + } + // BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes + link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0]; + link.body_I_body(0, 1) = 0.0; + link.body_I_body(0, 2) = 0.0; + link.body_I_body(1, 0) = 0.0; + link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1]; + link.body_I_body(1, 2) = 0.0; + link.body_I_body(2, 0) = 0.0; + link.body_I_body(2, 1) = 0.0; + link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2]; + // shift reference point to link origin (in joint axis) + mat33 tilde_r_com = tildeOperator(link.body_r_body_com); + link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com; + + if (verbose) { + id_printf("link %d: mass= %f, bt_inertia= [%f %f %f]\n" + "Io= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1], + bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1), + link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1), + link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1), + link.body_I_body(2, 2)); + } + // transform for vectors written in parent frame to this link's body-fixed frame + btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis(); + link.body_T_parent_ref(0, 0) = basis[0][0]; + link.body_T_parent_ref(0, 1) = basis[0][1]; + link.body_T_parent_ref(0, 2) = basis[0][2]; + link.body_T_parent_ref(1, 0) = basis[1][0]; + link.body_T_parent_ref(1, 1) = basis[1][1]; + link.body_T_parent_ref(1, 2) = basis[1][2]; + link.body_T_parent_ref(2, 0) = basis[2][0]; + link.body_T_parent_ref(2, 1) = basis[2][1]; + link.body_T_parent_ref(2, 2) = basis[2][2]; + if (verbose) { + id_printf("body_T_parent_ref= %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2], + basis[2][0], basis[2][1], basis[2][2]); + } + switch (bt_link.m_jointType) { + case btMultibodyLink::eRevolute: + link.joint_type = REVOLUTE; + if (verbose) { + id_printf("type= revolute\n"); + } + link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0]; + link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1]; + link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2]; + + // for revolute joints, m_eVector = parentComToThisPivotOffset + // m_dVector = thisPivotToThisComOffset + // from parent com to pivot, in parent frame + link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; + link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; + link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; + break; + case btMultibodyLink::ePrismatic: + link.joint_type = PRISMATIC; + if (verbose) { + id_printf("type= prismatic\n"); + } + link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0]; + link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1]; + link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2]; + + // for prismatic joints, eVector + // according to documentation : + // parentComToThisComOffset + // but seems to be: from parent's com to parent's + // pivot ?? + // m_dVector = thisPivotToThisComOffset + link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; + link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; + link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; + break; + case btMultibodyLink::eSpherical: + error_message("spherical joints not implemented\n"); + return -1; + case btMultibodyLink::ePlanar: + error_message("planar joints not implemented\n"); + return -1; + case btMultibodyLink::eFixed: + link.joint_type = FIXED; + // random unit vector + link.body_axis_of_motion(0) = 0; + link.body_axis_of_motion(1) = 0; + link.body_axis_of_motion(2) = 1; + + // for fixed joints, m_dVector = thisPivotToThisComOffset; + // m_eVector = parentComToThisPivotOffset; + link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; + link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; + link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; + break; + default: + error_message("unknown btMultiBody::eFeatherstoneJointType %d\n", + bt_link.m_jointType); + return -1; + } + if (link.parent_index > 0) { // parent body isn't the root + const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1); + // from parent pivot to parent com, in parent frame + link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0]; + link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1]; + link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2]; + } else { + // parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies, + // whose link frame is in the CoM (ie, no notion of a pivot point) + } + + if (verbose) { + id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0], + link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]); + } + } + + m_initialized = true; + + return 0; +} + +int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const { + if (false == m_initialized) { + error_message("btMultiBody not converted yet\n"); + return -1; + } + + *num_bodies = static_cast(m_data.size()); + return 0; +} + +int btMultiBodyTreeCreator::getBody(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 (false == m_initialized) { + error_message("MultiBodyTree not created yet\n"); + return -1; + } + + if (body_index < 0 || body_index >= static_cast(m_data.size())) { + error_message("index out of range (got %d but only %zu bodies)\n", body_index, + m_data.size()); + return -1; + } + + *parent_index = m_data[body_index].parent_index; + *joint_type = m_data[body_index].joint_type; + *parent_r_parent_body_ref = m_data[body_index].parent_r_parent_body_ref; + *body_T_parent_ref = m_data[body_index].body_T_parent_ref; + *body_axis_of_motion = m_data[body_index].body_axis_of_motion; + *mass = m_data[body_index].mass; + *body_r_body_com = m_data[body_index].body_r_body_com; + *body_I_body = m_data[body_index].body_I_body; + + *user_int = -1; + *user_ptr = 0x0; + + return 0; +} +} diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp new file mode 100644 index 000000000..1ae00ff79 --- /dev/null +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -0,0 +1,50 @@ +#ifndef BTMULTIBODYTREECREATOR_HPP_ +#define BTMULTIBODYTREECREATOR_HPP_ + +#include + +#include "BulletInverseDynamics/IDConfig.hpp" +#include "MultiBodyTreeCreator.hpp" +#include "BulletDynamics/Featherstone/btMultiBody.h" + +namespace btInverseDynamics { + +/// MultiBodyTreeCreator implementation for converting +/// a btMultiBody forward dynamics model into a MultiBodyTree inverse dynamics model +class btMultiBodyTreeCreator : public MultiBodyTreeCreator { +public: + /// ctor + btMultiBodyTreeCreator(); + /// dtor + ~btMultiBodyTreeCreator() {} + /// extract model data from a btMultiBody + /// @param btmb pointer to btMultiBody to convert + /// @param verbose if true, some information is printed + /// @return -1 on error, 0 on success + int createFromBtMultiBody(const btMultiBody *btmb, const bool verbose = false); + /// \copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int *num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(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 + struct LinkData { + 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; + }; + std::vector m_data; + bool m_initialized; +}; +} + +#endif // BTMULTIBODYTREECREATOR_HPP_ diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp new file mode 100644 index 000000000..21e693f62 --- /dev/null +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp @@ -0,0 +1,307 @@ +#include "invdyn_bullet_comparison.hpp" +#include +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" + +namespace btInverseDynamics { +int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose, + btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error, + double *acc_error) { +// call function and return -1 if it does, printing an error_message +#define RETURN_ON_FAILURE(x) \ + do { \ + if (-1 == x) { \ + error_message("calling " #x "\n"); \ + return -1; \ + } \ + } while (0) + + if (verbose) { + printf("\n ===================================== \n"); + } + vecx joint_forces(q.size()); + + // set positions and velocities for btMultiBody + // base link + mat33 world_T_base; + vec3 world_pos_base; + btTransform base_transform; + vec3 base_velocity; + vec3 base_angular_velocity; + + RETURN_ON_FAILURE(id_tree->setGravityInWorldFrame(gravity)); + RETURN_ON_FAILURE(id_tree->getBodyOrigin(0, &world_pos_base)); + RETURN_ON_FAILURE(id_tree->getBodyTransform(0, &world_T_base)); + RETURN_ON_FAILURE(id_tree->getBodyAngularVelocity(0, &base_angular_velocity)); + RETURN_ON_FAILURE(id_tree->getBodyLinearVelocityCoM(0, &base_velocity)); + + base_transform.setBasis(world_T_base); + base_transform.setOrigin(world_pos_base); + btmb->setBaseWorldTransform(base_transform); + btmb->setBaseOmega(base_angular_velocity); + btmb->setBaseVel(base_velocity); + btmb->setLinearDamping(0); + btmb->setAngularDamping(0); + + // remaining links + int q_index; + if (btmb->hasFixedBase()) { + q_index = 0; + } else { + q_index = 6; + } + if (verbose) { + printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs()); + } + for (int l = 0; l < btmb->getNumLinks(); l++) { + const btMultibodyLink &link = btmb->getLink(l); + if (verbose) { + printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount, + link.m_dofCount); + } + if (link.m_posVarCount == 1) { + btmb->setJointPosMultiDof(l, &q(q_index)); + btmb->setJointVelMultiDof(l, &u(q_index)); + if (verbose) { + printf("set q[%d]= %f, u[%d]= %f\n", q_index, q(q_index), q_index, u(q_index)); + } + q_index++; + } + } + // sanity check + if (q_index != q.size()) { + error_message("error in number of dofs for btMultibody and MultiBodyTree\n"); + return -1; + } + + // run inverse dynamics to determine joint_forces for given q, u, dot_u + if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces)) { + error_message("calculating inverse dynamics\n"); + return -1; + } + + // set up bullet forward dynamics model + btScalar dt = 0; + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + // this triggers switch between using either appliedConstraintForce or appliedForce + bool isConstraintPass = false; + // apply gravity forces for btMultiBody model. Must be done manually. + btmb->addBaseForce(btmb->getBaseMass() * gravity); + + for (int link = 0; link < btmb->getNumLinks(); link++) { + btmb->addLinkForce(link, gravity * btmb->getLinkMass(link)); + if (verbose) { + printf("link %d, applying gravity %f %f %f\n", link, + gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link), + gravity[2] * btmb->getLinkMass(link)); + } + } + + // apply generalized forces + if (btmb->hasFixedBase()) { + q_index = 0; + } else { + vec3 base_force; + base_force(0) = joint_forces(3); + base_force(1) = joint_forces(4); + base_force(2) = joint_forces(5); + + vec3 base_moment; + base_moment(0) = joint_forces(0); + base_moment(1) = joint_forces(1); + base_moment(2) = joint_forces(2); + + btmb->addBaseForce(world_T_base * base_force); + btmb->addBaseTorque(world_T_base * base_moment); + if (verbose) { + printf("base force from id: %f %f %f\n", joint_forces(3), joint_forces(4), + joint_forces(5)); + printf("base moment from id: %f %f %f\n", joint_forces(0), joint_forces(1), + joint_forces(2)); + } + q_index = 6; + } + + for (int l = 0; l < btmb->getNumLinks(); l++) { + const btMultibodyLink &link = btmb->getLink(l); + if (link.m_posVarCount == 1) { + if (verbose) { + printf("id:joint_force[%d]= %f, applied to link %d\n", q_index, + joint_forces(q_index), l); + } + btmb->addJointTorque(l, joint_forces(q_index)); + q_index++; + } + } + + // sanity check + if (q_index != q.size()) { + error_message("error in number of dofs for btMultibody and MultiBodyTree\n"); + return -1; + } + + // run forward kinematics & forward dynamics + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + btmb->forwardKinematics(world_to_local, local_origin); + btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); + + // read generalized accelerations back from btMultiBody + // the mapping from scratch variables to accelerations is taken from the implementation + // of stepVelocitiesMultiDof + btScalar *base_accel = &scratch_r[btmb->getNumDofs()]; + btScalar *joint_accel = base_accel + 6; + *acc_error = 0; + int dot_u_offset = 0; + if (btmb->hasFixedBase()) { + dot_u_offset = 0; + } else { + dot_u_offset = 6; + } + + if (true == btmb->hasFixedBase()) { + for (int i = 0; i < btmb->getNumDofs(); i++) { + if (verbose) { + printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], + dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i)); + } + *acc_error += std::pow(joint_accel[i] - dot_u(i + dot_u_offset), 2); + } + } else { + vec3 base_dot_omega; + vec3 world_dot_omega; + world_dot_omega(0) = base_accel[0]; + world_dot_omega(1) = base_accel[1]; + world_dot_omega(2) = base_accel[2]; + base_dot_omega = world_T_base.transpose() * world_dot_omega; + + // com happens to coincide with link origin here. If that changes, we need to calculate + // ddot_com + vec3 base_ddot_com; + vec3 world_ddot_com; + world_ddot_com(0) = base_accel[3]; + world_ddot_com(1) = base_accel[4]; + world_ddot_com(2) = base_accel[5]; + base_ddot_com = world_T_base.transpose()*world_ddot_com; + + for (int i = 0; i < 3; i++) { + if (verbose) { + printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i), + i, dot_u[i], base_dot_omega(i) - dot_u[i]); + } + *acc_error += std::pow(base_dot_omega(i) - dot_u(i), 2); + } + for (int i = 0; i < 3; i++) { + if (verbose) { + printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i), + i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]); + } + *acc_error += std::pow(base_ddot_com(i) - dot_u(i + 3), 2); + } + + for (int i = 0; i < btmb->getNumDofs(); i++) { + if (verbose) { + printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], + dot_u(i + 6), joint_accel[i] - dot_u(i + 6)); + } + *acc_error += std::pow(joint_accel[i] - dot_u(i + 6), 2); + } + } + *acc_error = std::sqrt(*acc_error); + if (verbose) { + printf("======dynamics-err: %e\n", *acc_error); + } + *pos_error = 0.0; + + { + mat33 world_T_body; + if (-1 == id_tree->getBodyTransform(0, &world_T_body)) { + error_message("getting transform for body %d\n", 0); + return -1; + } + vec3 world_com; + if (-1 == id_tree->getBodyCoM(0, &world_com)) { + error_message("getting com for body %d\n", 0); + return -1; + } + if (verbose) { + printf("id:com: %f %f %f\n", world_com(0), world_com(1), world_com(2)); + + printf("id:transform: %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + world_T_body(0, 0), world_T_body(0, 1), world_T_body(0, 2), world_T_body(1, 0), + world_T_body(1, 1), world_T_body(1, 2), world_T_body(2, 0), world_T_body(2, 1), + world_T_body(2, 2)); + } + } + + for (int l = 0; l < btmb->getNumLinks(); l++) { + const btMultibodyLink &bt_link = btmb->getLink(l); + + vec3 bt_origin = bt_link.m_cachedWorldTransform.getOrigin(); + mat33 bt_basis = bt_link.m_cachedWorldTransform.getBasis(); + if (verbose) { + printf("------------- link %d\n", l + 1); + printf("bt:com: %f %f %f\n", bt_origin(0), bt_origin(1), bt_origin(2)); + printf("bt:transform: %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + bt_basis(0, 0), bt_basis(0, 1), bt_basis(0, 2), bt_basis(1, 0), bt_basis(1, 1), + bt_basis(1, 2), bt_basis(2, 0), bt_basis(2, 1), bt_basis(2, 2)); + } + mat33 id_world_T_body; + vec3 id_world_com; + + if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body)) { + error_message("getting transform for body %d\n", l); + return -1; + } + if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com)) { + error_message("getting com for body %d\n", l); + return -1; + } + if (verbose) { + printf("id:com: %f %f %f\n", id_world_com(0), id_world_com(1), id_world_com(2)); + + printf("id:transform: %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + id_world_T_body(0, 0), id_world_T_body(0, 1), id_world_T_body(0, 2), + id_world_T_body(1, 0), id_world_T_body(1, 1), id_world_T_body(1, 2), + id_world_T_body(2, 0), id_world_T_body(2, 1), id_world_T_body(2, 2)); + } + vec3 diff_com = bt_origin - id_world_com; + mat33 diff_basis = bt_basis - id_world_T_body; + if (verbose) { + printf("diff-com: %e %e %e\n", diff_com(0), diff_com(1), diff_com(2)); + + printf("diff-transform: %e %e %e %e %e %e %e %e %e\n", diff_basis(0, 0), + diff_basis(0, 1), diff_basis(0, 2), diff_basis(1, 0), diff_basis(1, 1), + diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2)); + } + double total_pos_err = + std::sqrt(std::pow(diff_com(0), 2) + std::pow(diff_com(1), 2) + + std::pow(diff_com(2), 2) + std::pow(diff_basis(0, 0), 2) + + std::pow(diff_basis(0, 1), 2) + std::pow(diff_basis(0, 2), 2) + + std::pow(diff_basis(1, 0), 2) + std::pow(diff_basis(1, 1), 2) + + std::pow(diff_basis(1, 2), 2) + std::pow(diff_basis(2, 0), 2) + + std::pow(diff_basis(2, 1), 2) + std::pow(diff_basis(2, 2), 2)); + if (verbose) { + printf("======kin-pos-err: %e\n", total_pos_err); + } + if (total_pos_err > *pos_error) { + *pos_error = total_pos_err; + } + } + + return 0; +} +} diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.hpp b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp new file mode 100644 index 000000000..2a74af59e --- /dev/null +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp @@ -0,0 +1,34 @@ +#ifndef INVDYN_BULLET_COMPARISON_HPP +#define INVDYN_BULLET_COMPARISON_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 +/// 1. run inverse dynamics for (q, u, dot_u) to obtain joint forces f +/// 2. run forward dynamics (btMultiBody) for (q,u,f) to obtain dot_u_bullet +/// 3. compare dot_u with dot_u_bullet for cross check of forward and inverse dynamics computations +/// @param btmb the bullet forward dynamics model +/// @param id_tree the inverse dynamics model +/// @param q vector of generalized coordinates (matches id_tree) +/// @param u vector of generalized speeds (matches id_tree) +/// @param gravity gravitational acceleration in world frame +/// @param dot_u vector of generalized accelerations (matches id_tree) +/// @param gravity gravitational acceleration in world frame +/// @param base_fixed set base joint to fixed or +/// @param pos_error is set to the maximum of the euclidean norm of position+rotation errors of all +/// center of gravity positions and link frames +/// @param acc_error is set to the square root of the sum of squared differences of generalized +/// accelerations +/// computed in step 3 relative to dot_u +/// @return -1 on error, 0 on success +int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose, + btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error, + double *acc_error); +} +#endif // INVDYN_BULLET_COMPARISON_HPP diff --git a/Extras/InverseDynamics/premake4.lua b/Extras/InverseDynamics/premake4.lua new file mode 100644 index 000000000..0a524d46a --- /dev/null +++ b/Extras/InverseDynamics/premake4.lua @@ -0,0 +1,12 @@ + project "BulletInverseDynamicsUtils" + + kind "StaticLib" + + includedirs { + "../../src" + } + + files { + "*.cpp", + "*.hpp" + } diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp new file mode 100644 index 000000000..6df61071f --- /dev/null +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -0,0 +1,40 @@ +///@file Configuration for Inverse Dynamics Library, +/// such as choice of linear algebra library and underlying scalar type +#ifndef IDCONFIG_HPP_ +#define IDCONFIG_HPP_ +// If we have a custom configuration, compile without using other parts of bullet. +#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +#define BT_ID_WO_BULLET +#endif +// error messages +#include "IDErrorMessages.hpp" + +#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +#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) +#else +// Use default configuration with bullet's types +// Use the same scalar type as rest of bullet library +#include "LinearMath/btScalar.h" +typedef btScalar idScalar; +// use bullet types for arrays and array indices +#include "Bullet3Common/b3AlignedObjectArray.h" +// this is to make it work with C++2003, otherwise we could do this: +// template +// using idArray = b3AlignedObjectArray; +template +struct idArray { + typedef b3AlignedObjectArray type; +}; +typedef int idArrayIdx; +#define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR + +// use bullet's allocator functions +#define idMalloc btAllocFunc +#define idFree btFreeFunc + +#define ID_LINEAR_MATH_USE_BULLET +#include "details/IDLinearMathInterface.hpp" +#endif +#endif diff --git a/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/src/BulletInverseDynamics/IDConfigBuiltin.hpp new file mode 100644 index 000000000..2427c16f0 --- /dev/null +++ b/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -0,0 +1,36 @@ +///@file Configuration for Inverse Dynamics Library without external dependencies +#ifndef INVDYNCONFIG_BUILTIN_HPP_ +#define INVDYNCONFIG_BUILTIN_HPP_ +#ifdef BT_USE_DOUBLE_PRECISION +// choose double/single precision version +typedef double idScalar; +#else +typedef float idScalar; +#endif +// use std::vector for arrays +#include +// this is to make it work with C++2003, otherwise we could do this +// template +// using idArray = std::vector; +template +struct idArray { + typedef std::vector type; +}; +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*) {} + +#include "details/IDMatVec.hpp" +#endif diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp new file mode 100644 index 000000000..e04b149a6 --- /dev/null +++ b/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -0,0 +1,43 @@ +///@file Configuration for Inverse Dynamics Library with Eigen +#ifndef INVDYNCONFIG_EIGEN_HPP_ +#define INVDYNCONFIG_EIGEN_HPP_ +#ifdef BT_USE_DOUBLE_PRECISION +// choose double/single precision version +typedef double idScalar; +#else +typedef float idScalar; +#endif + +// use std::vector for arrays +#include +// this is to make it work with C++2003, otherwise we could do this +// template +// using idArray = std::vector; +template +struct idArray { + typedef std::vector type; +}; +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*) {} + +// Note on interfaces: +// Eigen::Matrix has data(), to get c-array storage +// HOWEVER: default storage is column-major! +#define ID_LINEAR_MATH_USE_EIGEN +#include "Eigen/Eigen" +#include "details/IDEigenInterface.hpp" +#endif diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp new file mode 100644 index 000000000..a66ea8323 --- /dev/null +++ b/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -0,0 +1,34 @@ +///@file error message utility functions +#ifndef IDUTILS_HPP_ +#define IDUTILS_HPP_ +#include +/// name of file being compiled, without leading path components +#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +#ifndef BT_ID_WO_BULLET +#include "Bullet3Common/b3Logging.h" +#define error_message(...) b3Error(__VA_ARGS__) +#define warning_message(...) b3Warning(__VA_ARGS__) +#define id_printf(...) b3Printf(__VA_ARGS__) +#else // BT_ID_WO_BULLET +#include +/// print error message with file/line information +#define error_message(...) \ + do { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +/// print warning message with file/line information +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define id_printf(...) printf(__VA_ARGS__) +#endif // BT_ID_WO_BULLET +#endif diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp new file mode 100644 index 000000000..a3daaf1f5 --- /dev/null +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -0,0 +1,371 @@ +#include "IDMath.hpp" + +#include +#include + +namespace btInverseDynamics { +static const idScalar kIsZero = 5 * std::numeric_limits::epsilon(); +// requirements for axis length deviation from 1.0 +// experimentally set from random euler angle rotation matrices +static const idScalar kAxisLengthEpsilon = 10 * kIsZero; + +void setZero(vec3 &v) { + v(0) = 0; + v(1) = 0; + v(2) = 0; +} + +void setZero(vecx &v) { + for (int i = 0; i < v.size(); i++) { + v(i) = 0; + } +} + +void setZero(mat33 &m) { + m(0, 0) = 0; + m(0, 1) = 0; + m(0, 2) = 0; + m(1, 0) = 0; + m(1, 1) = 0; + m(1, 2) = 0; + m(2, 0) = 0; + m(2, 1) = 0; + m(2, 2) = 0; +} + +idScalar maxAbs(const vecx &v) { + idScalar result = 0.0; + for (int i = 0; i < v.size(); i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; +} + +idScalar maxAbs(const vec3 &v) { + idScalar result = 0.0; + for (int i = 0; i < 3; i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; +} + +mat33 transformX(const idScalar &alpha) { + mat33 T; + const idScalar cos_alpha = std::cos(alpha); + const idScalar sin_alpha = std::sin(alpha); + // [1 0 0] + // [0 c s] + // [0 -s c] + T(0, 0) = 1.0; + T(0, 1) = 0.0; + T(0, 2) = 0.0; + + T(1, 0) = 0.0; + T(1, 1) = cos_alpha; + T(1, 2) = sin_alpha; + + T(2, 0) = 0.0; + T(2, 1) = -sin_alpha; + T(2, 2) = cos_alpha; + + return T; +} + +mat33 transformY(const idScalar &beta) { + mat33 T; + const idScalar cos_beta = std::cos(beta); + const idScalar sin_beta = std::sin(beta); + // [c 0 -s] + // [0 1 0] + // [s 0 c] + T(0, 0) = cos_beta; + T(0, 1) = 0.0; + T(0, 2) = -sin_beta; + + T(1, 0) = 0.0; + T(1, 1) = 1.0; + T(1, 2) = 0.0; + + T(2, 0) = sin_beta; + T(2, 1) = 0.0; + T(2, 2) = cos_beta; + + return T; +} + +mat33 transformZ(const idScalar &gamma) { + mat33 T; + const idScalar cos_gamma = std::cos(gamma); + const idScalar sin_gamma = std::sin(gamma); + // [ c s 0] + // [-s c 0] + // [ 0 0 1] + T(0, 0) = cos_gamma; + T(0, 1) = sin_gamma; + T(0, 2) = 0.0; + + T(1, 0) = -sin_gamma; + T(1, 1) = cos_gamma; + T(1, 2) = 0.0; + + T(2, 0) = 0.0; + T(2, 1) = 0.0; + T(2, 2) = 1.0; + + return T; +} + +mat33 tildeOperator(const vec3 &v) { + mat33 m; + m(0, 0) = 0.0; + m(0, 1) = -v(2); + m(0, 2) = v(1); + m(1, 0) = v(2); + m(1, 1) = 0.0; + m(1, 2) = -v(0); + m(2, 0) = -v(1); + m(2, 1) = v(0); + m(2, 2) = 0.0; + return m; +} + +void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { + const idScalar sa = std::sin(alpha); + const idScalar ca = std::cos(alpha); + const idScalar st = std::sin(theta); + const idScalar ct = std::cos(theta); + + (*r)(0) = a; + (*r)(1) = -sa * d; + (*r)(2) = ca * d; + + (*T)(0, 0) = ct; + (*T)(0, 1) = -st; + (*T)(0, 2) = 0.0; + + (*T)(1, 0) = st * ca; + (*T)(1, 1) = ct * ca; + (*T)(1, 2) = -sa; + + (*T)(2, 0) = st * sa; + (*T)(2, 1) = ct * sa; + (*T)(2, 2) = ca; +} + +void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { + const idScalar c = cos(angle); + const idScalar s = -sin(angle); + const idScalar one_m_c = 1.0 - c; + + const idScalar &x = axis(0); + const idScalar &y = axis(1); + const idScalar &z = axis(2); + + (*T)(0, 0) = x * x * one_m_c + c; + (*T)(0, 1) = x * y * one_m_c - z * s; + (*T)(0, 2) = x * z * one_m_c + y * s; + + (*T)(1, 0) = x * y * one_m_c + z * s; + (*T)(1, 1) = y * y * one_m_c + c; + (*T)(1, 2) = y * z * one_m_c - x * s; + + (*T)(2, 0) = x * z * one_m_c - y * s; + (*T)(2, 1) = y * z * one_m_c + x * s; + (*T)(2, 2) = z * z * one_m_c + c; +} + +bool isPositiveDefinite(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) <= 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; +} + +bool isPositiveSemiDefinite(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) < 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; +} + +bool isPositiveSemiDefiniteFuzzy(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) < -kIsZero) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) { + return false; + } + return true; +} + +idScalar determinant(const mat33 &m) { + return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - + m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2); +} + +bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) { + // TODO(Thomas) do we really want this? + // in cases where the inertia tensor about the center of mass is zero, + // the determinant of the inertia tensor about the joint axis is almost + // zero and can have a very small negative value. + if (!isPositiveSemiDefiniteFuzzy(I)) { + error_message("invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + + return false; + } + + // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) + if (!has_fixed_joint) { + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(1, 1) + I(2, 2) < I(0, 0)) { + error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + } + // check positive/zero diagonal elements + for (int i = 0; i < 3; i++) { + if (I(i, i) < 0) { // accept zero + error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); + return false; + } + } + // check symmetry + if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " + "%e\n", + index, I(1, 0) - I(0, 1)); + return false; + } + if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " + "%e\n", + index, I(2, 0) - I(0, 2)); + return false; + } + if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) { + error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, + I(1, 2) - I(2, 1)); + return false; + } + return true; +} + +bool isValidTransformMatrix(const mat33 &m) { +#define print_mat(x) \ + error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) + + // check for unit length column vectors + for (int i = 0; i < 3; i++) { + const idScalar length_minus_1 = + std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0); + if (length_minus_1 > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (column %d not unit length)\n" + "column = [%.18e %.18e %.18e]\n" + "length-1.0= %.18e\n", + i, m(0, i), m(1, i), m(2, i), length_minus_1); + print_mat(m); + return false; + } + } + // check for orthogonal column vectors + if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n"); + print_mat(m); + return false; + } + if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + // check determinant (rotation not reflection) + if (determinant(m) <= 0) { + error_message("Not a valid rotation matrix (determinant <=0)\n"); + print_mat(m); + return false; + } + return true; +} + +bool isUnitVector(const vec3 &vector) { + return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < + kIsZero; +} + +vec3 rpyFromMatrix(const mat33 &rot) { + vec3 rpy; + rpy(2) = std::atan2(-rot(1, 0), rot(0, 0)); + rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0)); + rpy(0) = std::atan2(-rot(2, 0), rot(2, 2)); + return rpy; +} +} diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp new file mode 100644 index 000000000..97e60c120 --- /dev/null +++ b/src/BulletInverseDynamics/IDMath.hpp @@ -0,0 +1,88 @@ +/// @file Math utility functions used in inverse dynamics library. +/// Defined here as they may not be provided by the math library. + +#ifndef IDMATH_HPP_ +#define IDMATH_HPP_ +#include "IDConfig.hpp" + +namespace btInverseDynamics { +/// set all elements to zero +void setZero(vec3& v); +/// set all elements to zero +void setZero(vecx& v); +/// set all elements to zero +void setZero(mat33& m); + +/// return maximum absolute value +idScalar maxAbs(const vecx& v); +#ifndef ID_LINEAR_MATH_USE_EIGEN +/// return maximum absolute value +idScalar maxAbs(const vec3& v); +#endif //ID_LINEAR_MATH_USE_EIGEN +/// 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); + +/// Check if a 3x3 matrix is positive definite +/// @param m a 3x3 matrix +/// @return true if m>0, false otherwise +bool isPositiveDefinite(const mat33& m); + +/// Check if a 3x3 matrix is positive semi definite +/// @param m a 3x3 matrix +/// @return true if m>=0, false otherwise +bool isPositiveSemiDefinite(const mat33& m); +/// Check if a 3x3 matrix is positive semi definite within numeric limits +/// @param m a 3x3 matrix +/// @return true if m>=-eps, false otherwise +bool isPositiveSemiDefiniteFuzzy(const mat33& m); + +/// Determinant of 3x3 matrix +/// NOTE: implemented here for portability, as determinant operation +/// will be implemented differently for various matrix/vector libraries +/// @param m a 3x3 matrix +/// @return det(m) +idScalar determinant(const mat33& m); + +/// Test if a 3x3 matrix satisfies some properties of inertia matrices +/// @param I a 3x3 matrix +/// @param index body index (for error messages) +/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted +/// @return true if I satisfies inertia matrix properties, false otherwise. +bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint); + +/// Check if a 3x3 matrix is a valid transform (rotation) matrix +/// @param m a 3x3 matrix +/// @return true if m is a rotation matrix, false otherwise +bool isValidTransformMatrix(const mat33& m); +/// Transform matrix from parent to child frame, +/// when the child frame is rotated about @param axis by @angle +/// (mathematically positive) +/// @param axis the axis of rotation +/// @param angle rotation angle +/// @param T pointer to transform matrix +void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T); + +/// Check if this is a unit vector +/// @param vector +/// @return true if |vector|=1 within numeric limits +bool isUnitVector(const vec3& vector); + +/// @input a vector in R^3 +/// @returns corresponding spin tensor +mat33 tildeOperator(const vec3& v); +/// @param alpha angle in radians +/// @returns transform matrix for ratation with @param alpha about x-axis +mat33 transformX(const idScalar& alpha); +/// @param beta angle in radians +/// @returns transform matrix for ratation with @param beta about y-axis +mat33 transformY(const idScalar& beta); +/// @param gamma angle in radians +/// @returns transform matrix for ratation with @param gamma about z-axis +mat33 transformZ(const idScalar& gamma); +///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix +/// @param rot rotation matrix +/// @returns x-y-z Euler angles +vec3 rpyFromMatrix(const mat33&rot); +} +#endif // IDMATH_HPP_ diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp new file mode 100644 index 000000000..3de0d8e4f --- /dev/null +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -0,0 +1,331 @@ +#include "MultiBodyTree.hpp" + +#include +#include +#include + +#include "IDMath.hpp" +#include "details/MultiBodyTreeImpl.hpp" +#include "details/MultiBodyTreeInitCache.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::MultiBodyTree() + : m_is_finalized(false), + m_mass_parameters_are_valid(true), + m_accept_invalid_mass_parameters(false), + m_impl(0x0), + m_init_cache(0x0) { + m_init_cache = new InitCache(); +} + +MultiBodyTree::~MultiBodyTree() { + delete m_impl; + delete m_init_cache; +} + +void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { + m_accept_invalid_mass_parameters = flag; +} + +bool MultiBodyTree::getAcceptInvalidMassProperties() const { + return m_accept_invalid_mass_parameters; +} + +int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { + return m_impl->getBodyOrigin(body_index, world_origin); +} + +int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const { + return m_impl->getBodyCoM(body_index, world_com); +} + +int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const { + return m_impl->getBodyTransform(body_index, world_T_body); +} +int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const { + return m_impl->getBodyAngularVelocity(body_index, world_omega); +} +int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const { + return m_impl->getBodyLinearVelocity(body_index, world_velocity); +} + +int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const { + return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); +} + +int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const { + return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); +} +int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const { + return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); +} + +void MultiBodyTree::printTree() { m_impl->printTree(); } +void MultiBodyTree::printTreeData() { m_impl->printTreeData(); } + +int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; } + +int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; } + +int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u, + vecx *joint_forces) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + error_message("error in inverse dynamics calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, + const bool initialize_matrix, + const bool set_lower_triangular_matrix, matxx *mass_matrix) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == + m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, + set_lower_triangular_matrix, mass_matrix)) { + error_message("error in mass matrix calculation\n"); + return -1; + } + return 0; +} +int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { + return calculateMassMatrix(q, true, true, true, mass_matrix); +} +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, + const vec3 &body_r_body_com, const mat33 &body_I_body, + const int user_int, void *user_ptr) { + if (body_index < 0) { + error_message("body index must be positive (got %d)\n", body_index); + return -1; + } + vec3 body_axis_of_motion(body_axis_of_motion_); + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + // check if axis is unit vector + if (!isUnitVector(body_axis_of_motion)) { + warning_message( + "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", + body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); + idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) + + std::pow(body_axis_of_motion(1), 2) + + std::pow(body_axis_of_motion(2), 2)); + if (length < std::sqrt(std::numeric_limits::min())) { + error_message("axis of motion vector too short (%e)\n", length); + return -1; + } + body_axis_of_motion = (1.0 / length) * body_axis_of_motion; + } + break; + case FIXED: + break; + case FLOATING: + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + // sanity check for mass properties. Zero mass is OK. + if (mass < 0) { + m_mass_parameters_are_valid = false; + error_message("Body %d has invalid mass %e\n", body_index, mass); + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) { + m_mass_parameters_are_valid = false; + // error message printed in function call + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + if (!isValidTransformMatrix(body_T_parent_ref)) { + return -1; + } + + return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, + body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr); +} + +int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { + return m_impl->getParentIndex(body_index, parent_index); +} + +int MultiBodyTree::getUserInt(const int body_index, int *user_int) const { + return m_impl->getUserInt(body_index, user_int); +} + +int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const { + return m_impl->getUserPtr(body_index, user_ptr); +} + +int MultiBodyTree::setUserInt(const int body_index, const int user_int) { + return m_impl->setUserInt(body_index, user_int); +} + +int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) { + return m_impl->setUserPtr(body_index, user_ptr); +} + +int MultiBodyTree::finalize() { + const int &num_bodies = m_init_cache->numBodies(); + const int &num_dofs = m_init_cache->numDoFs(); + + // 1 allocate internal MultiBody structure + m_impl = new MultiBodyImpl(num_bodies, num_dofs); + + // 2 build new index set assuring index(parent) < index(child) + if (-1 == m_init_cache->buildIndexSets()) { + return -1; + } + m_init_cache->getParentIndexArray(&m_impl->m_parent_index); + + // 3 setup internal kinematic and dynamic data + for (int index = 0; index < num_bodies; index++) { + InertiaData inertia; + JointData joint; + if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + return -1; + } + if (-1 == m_init_cache->getJointData(index, &joint)) { + return -1; + } + + RigidBody &rigid_body = m_impl->m_body_list[index]; + + rigid_body.m_mass = inertia.m_mass; + rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; + rigid_body.m_body_I_body = inertia.m_body_I_body; + rigid_body.m_joint_type = joint.m_type; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_joint_type = joint.m_type; + + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized + // matrices. + switch (rigid_body.m_joint_type) { + case REVOLUTE: + rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case PRISMATIC: + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); + break; + case FIXED: + // NOTE/TODO: dimension really should be zero .. + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case FLOATING: + // NOTE/TODO: this is not really correct. + // the Jacobians should be 3x3 matrices here ! + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + default: + error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + return -1; + } + } + + // 4 assign degree of freedom indices & build per-joint-type index arrays + if (-1 == m_impl->generateIndexSets()) { + error_message("generating index sets\n"); + return -1; + } + + // 5 do some pre-computations .. + m_impl->calculateStaticData(); + + // 6. make sure all user forces are set to zero, as this might not happen + // in the vector ctors. + m_impl->clearAllUserForcesAndMoments(); + + m_is_finalized = true; + return 0; +} + +int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { + return m_impl->setGravityInWorldFrame(gravity); +} + +int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { + return m_impl->getJointType(body_index, joint_type); +} + +int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const { + return m_impl->getJointTypeStr(body_index, joint_type); +} + +int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const { + return m_impl->getDoFOffset(body_index, q_offset); +} + +int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { + return m_impl->setBodyMass(body_index, mass); +} + +int MultiBodyTree::setBodyFirstMassMoment(const int body_index, vec3 first_mass_moment) { + return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); +} + +int MultiBodyTree::setBodySecondMassMoment(const int body_index, mat33 second_mass_moment) { + return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); +} + +int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const { + return m_impl->getBodyMass(body_index, mass); +} + +int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const { + return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); +} + +int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const { + return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); +} + +void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); } + +int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) { + return m_impl->addUserForce(body_index, body_force); +} + +int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) { + return m_impl->addUserMoment(body_index, body_moment); +} + +} diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp new file mode 100644 index 000000000..833baf1d3 --- /dev/null +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -0,0 +1,308 @@ +#ifndef MULTIBODYTREE_HPP_ +#define MULTIBODYTREE_HPP_ + +#include "IDMath.hpp" + +namespace btInverseDynamics { + +/// Enumeration of supported joint types +enum JointType { + /// no degree of freedom, moves with parent + FIXED = 0, + /// one rotational degree of freedom relative to parent + REVOLUTE, + /// one translational degree of freedom relative to parent + PRISMATIC, + /// six degrees of freedom relative to parent + FLOATING +}; + +/// Interface class for calculating inverse dynamics for tree structured +/// multibody systems +/// +/// Note on degrees of freedom +/// The q vector contains the generalized coordinate set defining the tree's configuration. +/// Every joint adds elements that define the corresponding link's frame pose relative to +/// its parent. For the joint types that is: +/// - FIXED: none +/// - REVOLUTE: angle of rotation [rad] +/// - PRISMATIC: displacement [m] +/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] +/// (in that order) +/// The u vector contains the generalized speeds, which are +/// - FIXED: none +/// - REVOLUTE: time derivative of angle of rotation [rad/s] +/// - PRISMATIC: time derivative of displacement [m/s] +/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) +/// and time derivative of displacement in parent frame [m/s] +/// +/// The q and u vectors are obtained by stacking contributions of all bodies in one +/// vector in the order of body indices. +/// +/// Note on generalized forces: analogous to u, i.e., +/// - FIXED: none +/// - REVOLUTE: moment [Nm], about joint axis +/// - PRISMATIC: force [N], along joint axis +/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame +/// (in that order) +/// +/// TODO - force element interface (friction, springs, dampers, etc) +/// - gears and motor inertia +class MultiBodyTree { +public: + /// The contructor. + /// Initialization & allocation is via addBody and buildSystem calls. + MultiBodyTree(); + /// the destructor. This also deallocates all memory + ~MultiBodyTree(); + + /// Add body to the system. this allocates memory and not real-time safe. + /// This only adds the data to an initial cache. After all bodies have been + /// added, + /// the system is setup using the buildSystem call + /// @param body_index index of the body to be added. Must >=0, =dim(u) + /// @param dot_u time derivative of u + /// @param joint_forces this is where the resulting joint forces will be + /// stored. dim(joint_forces) = dim(u) + /// @return 0 on success, -1 on error + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + /// Calculate joint space mass matrix + /// @param q generalized coordinates + /// @param initialize_matrix if true, initialize mass matrix with zero. + /// If mass_matrix is initialized to zero externally and only used + /// for mass matrix computations for the same system, it is safe to + /// set this to false. + /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix + /// is also populated, otherwise not. + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + + /// Calculate joint space mass matrix. + /// This version will update kinematics, initialize all mass_matrix elements to zero and + /// populate all mass matrix entries. + /// @param q generalized coordinates + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, matxx* mass_matrix); + + /// set gravitational acceleration + /// the default is [0;0;-9.8] in the world frame + /// @param gravity the gravitational acceleration in world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// returns number of bodies in tree + int numBodies() const; + /// returns number of mechanical degrees of freedom (dimension of q-vector) + int numDoFs() const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// get center of mass of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyCoM(const int body_index, vec3* world_com) const; + /// get transform from of a body-fixed frame to the world frame + /// @param body_index index for frame/body + /// @param world_T_body pointer for return data + /// @return 0 on success, -1 on error + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// get absolute angular velocity for a body, represented in the world frame + /// @param body_index index for frame/body + /// @param world_omega pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// get linear velocity of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_velocity pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// get linear velocity of a body's CoM, represented in world frame + /// (not required for inverse dynamics, provided for convenience) + /// @param body_index index for frame/body + /// @param world_vel_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// get origin of a body-fixed frame, represented in world frame + /// NOTE: this will include the gravitational acceleration, so the actual acceleration is + /// obtainened by setting gravitational acceleration to zero, or subtracting it. + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// returns the (internal) index of body + /// @param body_index is the index of a body (internal: TODO: fix/clarify + /// indexing!) + /// @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; + /// get joint type + /// @param body_index index of the body + /// @param joint_type the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointType(const int body_index, JointType* joint_type) const; + /// get joint type as string + /// @param body_index index of the body + /// @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 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 + /// @return -1 on error, 0 on success + int getDoFOffset(const int body_index, int* q_offset) const; + /// get user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int getUserInt(const int body_index, int* user_int) const; + /// get user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int getUserPtr(const int body_index, void** user_ptr) const; + /// set user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int setUserInt(const int body_index, const int user_int); + /// set user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int setUserPtr(const int body_index, void* const user_ptr); + /// set mass for a body + /// @param body_index index of the body + /// @param mass the mass to set + /// @return 0 on success, -1 on failure + int setBodyMass(const int body_index, const idScalar mass); + /// set first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_mass_moment the vector to set + /// @return 0 on success, -1 on failure + int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + /// set second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + /// get mass for a body + /// @param body_index index of the body + /// @param mass the mass + /// @return 0 on success, -1 on failure + int getBodyMass(const int body_index, idScalar* mass) const; + /// get first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_moment the vector + /// @return 0 on success, -1 on failure + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + /// get second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// set all user forces and moments to zero + void clearAllUserForcesAndMoments(); + /// Add an external force to a body, acting at the origin of the body-fixed frame. + /// Calls to addUserForce are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_force the force represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserForce(const int body_index, const vec3& body_force); + /// Add an external moment to a body. + /// Calls to addUserMoment are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_moment the moment represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserMoment(const int body_index, const vec3& body_moment); + +private: + // flag indicating if system has been initialized + bool m_is_finalized; + // flag indicating if mass properties are physically valid + bool m_mass_parameters_are_valid; + // flag defining if unphysical mass parameters are accepted + bool m_accept_invalid_mass_parameters; + // This struct implements the inverse dynamics calculations + class MultiBodyImpl; + MultiBodyImpl* m_impl; + // cache data structure for initialization + class InitCache; + InitCache* m_init_cache; +}; +} // namespace btInverseDynamics + +#endif // MULTIBODYTREE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDEigenInterface.hpp b/src/BulletInverseDynamics/details/IDEigenInterface.hpp new file mode 100644 index 000000000..ec6136aa5 --- /dev/null +++ b/src/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -0,0 +1,17 @@ +#ifndef INVDYNEIGENINTERFACE_HPP_ +#define INVDYNEIGENINTERFACE_HPP_ +#include "../IDConfig.hpp" +namespace btInverseDynamics { +#ifdef BT_USE_DOUBLE_PRECISION +typedef Eigen::VectorXd vecx; +typedef Eigen::Vector3d vec3; +typedef Eigen::Matrix3d mat33; +typedef Eigen::MatrixXd matxx; +#else +typedef Eigen::VectorXf vecx; +typedef Eigen::Vector3f vec3; +typedef Eigen::Matrix3f mat33; +typedef Eigen::MatrixXf matxx; +#endif // +} +#endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp new file mode 100644 index 000000000..2209121ba --- /dev/null +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -0,0 +1,112 @@ +#ifndef IDLINEARMATHINTERFACE_HPP_ +#define IDLINEARMATHINTERFACE_HPP_ + +#include + +#include "../IDConfig.hpp" + +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btMatrixX.h" + +namespace btInverseDynamics { +class vec3; +class vecx; +class mat33; +typedef btMatrixX matxx; + +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]; } + const int size() const { return 3; } + const vec3& operator=(const btVector3& rhs) { + *static_cast(this) = rhs; + return *this; + } +}; + +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) { + *static_cast(this) = rhs; + return *this; + } + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator/(const mat33& a, const idScalar& s); +}; + +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 { +public: + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) { + *static_cast(this) = rhs; + return *this; + } + + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); + + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend 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++) { + 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) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + 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) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + 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) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) / s; + } + + return result; +} +} + +#endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp new file mode 100644 index 000000000..961abade8 --- /dev/null +++ b/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -0,0 +1,326 @@ +/// @file Built-In Matrix-Vector functions +#ifndef IDMATVEC_HPP_ +#define IDMATVEC_HPP_ + +#include + +namespace btInverseDynamics { +class vec3; +class vecx; +class mat33; +class matxx; + +/// 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 { +public: + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int size() const { return 3; } + const vec3& operator=(const vec3& rhs); + const vec3& operator+=(const vec3& b); + const vec3& operator-=(const vec3& b); + vec3 cross(const vec3& b) const; + idScalar dot(const vec3& b) const; + + friend vec3 operator*(const mat33& a, const vec3& b); + friend vec3 operator*(const vec3& a, const idScalar& s); + friend vec3 operator*(const idScalar& s, const vec3& a); + + friend vec3 operator+(const vec3& a, const vec3& b); + friend vec3 operator-(const vec3& a, const vec3& b); + friend vec3 operator/(const vec3& a, const idScalar& s); + +private: + idScalar m_data[3]; +}; + +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]; } + const mat33& operator=(const mat33& rhs); + mat33 transpose() const; + const mat33& operator+=(const mat33& b); + const mat33& operator-=(const mat33& b); + + friend mat33 operator*(const mat33& a, const mat33& b); + friend vec3 operator*(const mat33& a, const vec3& b); + friend mat33 operator*(const mat33& a, const idScalar& s); + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator+(const mat33& a, const mat33& b); + friend mat33 operator-(const mat33& a, const mat33& b); + friend mat33 operator/(const mat33& a, const idScalar& s); + +private: + // layout is [0,1,2;3,4,5;6,7,8] + idScalar m_data[9]; +}; + +class vecx { +public: + vecx(int size) : m_size(size) { + m_data = static_cast(idMalloc(sizeof(idScalar) * size)); + } + ~vecx() { idFree(m_data); } + const vecx& operator=(const vecx& rhs); + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int& size() const { return m_size; } + + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); + + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); + +private: + int m_size; + idScalar* m_data; +}; + +class matxx { +public: + 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]; } + const int& rows() const { return m_rows; } + const int& cols() const { return m_cols; } + +private: + int m_rows; + int m_cols; + idScalar* m_data; +}; + +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 { + 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]; + result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; + + return result; +} + +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) { + memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); + } + return *this; +} +inline mat33 mat33::transpose() const { + mat33 result; + result.m_data[0] = m_data[0]; + result.m_data[1] = m_data[3]; + result.m_data[2] = m_data[6]; + result.m_data[3] = m_data[1]; + result.m_data[4] = m_data[4]; + result.m_data[5] = m_data[7]; + result.m_data[6] = m_data[2]; + result.m_data[7] = m_data[5]; + result.m_data[8] = m_data[8]; + + return result; +} + +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]; + result.m_data[1] = + a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; + result.m_data[2] = + a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; + result.m_data[3] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; + result.m_data[4] = + a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; + result.m_data[5] = + a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; + result.m_data[6] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; + result.m_data[7] = + a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; + result.m_data[8] = + a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; + + return result; +} + +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++) { + m_data[i] -= b.m_data[i]; + } + return *this; +} + +inline vec3 operator*(const mat33& a, const vec3& b) { + vec3 result; + + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; + result.m_data[1] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; + result.m_data[2] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; + + return result; +} + +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++) { + m_data[i] -= b.m_data[i]; + } + return *this; +} + +inline mat33 operator*(const mat33& a, const idScalar& s) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} + +inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } + +inline vec3 operator*(const vec3& a, const idScalar& s) { + vec3 result; + 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) { + mat33 result; + 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) { + vec3 result; + 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) { + mat33 result; + 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) { + vec3 result; + 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) { + mat33 result; + 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) { + vec3 result; + 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()) { + error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); + abort(); + } + if (&rhs != this) { + memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); + } + return *this; +} +inline vecx operator*(const vecx& a, const idScalar& s) { + vecx result(a.size()); + 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) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + 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) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + 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) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] / s; + } + + return result; +} +} +#endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp new file mode 100644 index 000000000..53ef4503d --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -0,0 +1,844 @@ +#include "MultiBodyTreeImpl.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) + : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) { + m_body_list.resize(num_bodies_); + m_parent_index.resize(num_bodies_); + m_child_indices.resize(num_bodies_); + m_user_int.resize(num_bodies_); + m_user_ptr.resize(num_bodies_); + + m_world_gravity(0) = 0.0; + m_world_gravity(1) = 0.0; + m_world_gravity(2) = -9.8; +} + +const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { + switch (type) { + case FIXED: + return "fixed"; + case REVOLUTE: + return "revolute"; + case PRISMATIC: + return "prismatic"; + case FLOATING: + return "floating"; + } + return "error: invalid"; +} + +inline void indent(const int &level) { + for (int j = 0; j < level; j++) + id_printf(" "); // indent +} + +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++) { + RigidBody &body = m_body_list[i]; + id_printf("body: %d\n", static_cast(i)); + id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); + id_printf("q_index= %d\n", body.m_q_index); + id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); + id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); + + 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("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) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + error_message("unknown joint type %d\n", type); + return 0; +} + +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(); + + indentation += 2; + int count = 0; + + 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, + jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, + m_body_list[index].m_q_index, + m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); + // first grandchild + printTree(child_index, indentation); + } +} + +int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { + m_world_gravity = gravity; + return 0; +} + +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++) { + RigidBody &body = m_body_list[i]; + body.m_q_index = -1; + switch (body.m_joint_type) { + case REVOLUTE: + m_body_revolute_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case PRISMATIC: + m_body_prismatic_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case FIXED: + // do nothing + break; + case FLOATING: + m_body_floating_list.push_back(i); + body.m_q_index = q_index; + q_index += 6; + break; + default: + error_message("unsupported joint type %d\n", body.m_joint_type); + return -1; + } + } + // sanity check + if (q_index != m_num_dofs) { + 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++) { + const int &parent = m_parent_index[child]; + if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { + m_child_indices[parent].push_back(child); + } else { + if (-1 == parent) { + // multiple bodies are directly linked to the environment, ie, not a single root + error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } else { + // should never happen + error_message( + "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", + child, parent, static_cast(m_parent_index.size())); + } + return -1; + } + } + + return 0; +} + +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++) { + RigidBody &body = m_body_list[i]; + switch (body.m_joint_type) { + case REVOLUTE: + 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; + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + break; + case PRISMATIC: + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + break; + case FIXED: + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_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; + case FLOATING: + // no static data + break; + } + } +} + +int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, + 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) { + 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(q.size()), static_cast(u.size()), + static_cast(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); + } + + 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 = + body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + + body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - + body.m_body_moment_user; + body.m_eom_lhs_translational = + body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + + body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - + body.m_body_force_user; + } + + // 4. 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, + // but that would make changing masses online harder (eg, payload masses + // added with fixed joints to a gripper) + // 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--) { + // 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++) { + 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; + sum_f_children -= child_joint_force_in_this_frame; + sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + + child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); + } + RigidBody &body = m_body_list[body_idx]; + + body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; + body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; + } + + // 4. Calculate Joint forces. + // 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++) { + 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++) { + 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++) { + 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); + (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); + + (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); + (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); + (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); + } + + return 0; +} + +static inline void setSixDoFJacobians(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; + // translational part + case 3: + setZero(Jac_JR); + Jac_JT(0) = 1; + Jac_JT(1) = 0; + Jac_JT(2) = 0; + break; + case 4: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 1; + Jac_JT(2) = 0; + break; + case 5: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 0; + Jac_JT(2) = 1; + break; + } +} + +static inline int jointNumDoFs(const JointType &type) { + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + // this should never happen + error_message("invalid joint type\n"); + // TODO add configurable abort/crash function + abort(); +} + +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. + +// 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" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), + static_cast(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++) { + setMassMatrixElem(i, j, 0.0); + } + } + } + + if (update_kinematics) { + // 1. update relative kinematics + // 1.1 for revolute joints + 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; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); + 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++) { + 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); + } + // 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; + } + } + 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 + body.m_subtree_mass = body.m_mass; + 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++) { + RigidBody &child = m_body_list[m_child_indices[i][c]]; + mat33 body_T_child = child.m_body_T_parent.transpose(); + + body.m_subtree_mass += child.m_subtree_mass; + body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + + child.m_parent_pos_parent_body * child.m_subtree_mass; + 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) { + // 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 + // origin) + vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; + mat33 tilde_r_child_com = tildeOperator(r_com); + mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); + body.m_body_subtree_I_body += + child.m_subtree_mass * + (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); + } + } + } + + for (int i = m_body_list.size() - 1; i >= 0; i--) { + const RigidBody &body = m_body_list[i]; + + // determine DoF-range for body + const int q_index_min = body.m_q_index; + const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; + // loop over the DoFs used by this body + // 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--) { + // set jacobians for 6-DoF joints + if (FLOATING == body.m_joint_type) { + setSixDoFJacobians(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); + 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)); + + // 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) { + error_message("??\n"); + return -1; + } + 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); + } + // 2. ancestor dofs + int child_idx = i; + int parent_idx = m_parent_index[i]; + while (parent_idx >= 0) { + const RigidBody &child_body = m_body_list[child_idx]; + const RigidBody &parent_body = m_body_list[parent_idx]; + + const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); + body_eom_rot = parent_T_child * body_eom_rot; + body_eom_trans = parent_T_child * body_eom_trans; + body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); + + const int parent_body_q_index_min = parent_body.m_q_index; + const int parent_body_q_index_max = + 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--) { + // set jacobians for 6-DoF joints + 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); + setMassMatrixElem(col, row, Mrc); + } + + child_idx = parent_idx; + parent_idx = m_parent_index[child_idx]; + } + } + } + } + + 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)); + } + } + } + +#undef setMassMatrixElem + return 0; +} + +// utility macro +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do { \ + if (index < 0 || index >= m_num_bodies) { \ + 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) { + 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 { + 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 { + 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) { + 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) { + 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 { + 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 { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + 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 { + *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 { + 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 { + 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 { + 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; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, + 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) { + com = body.m_body_mass_com / body.m_mass; + } else { + com(0) = 0; + com(1) = 0; + com(2) = 0; + } + + *world_velocity = + body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, + 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 { + 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 { + 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 { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); + 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; + return 0; +} + +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) { + 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) { + 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 { + 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 { + 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 { + 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++) { + 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) { + 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) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_moment_user += body_moment; + return 0; +} + +} diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp new file mode 100644 index 000000000..63d8945eb --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -0,0 +1,236 @@ +// The structs and classes defined here provide a basic inverse fynamics implementation used +// by MultiBodyTree +// User interaction should be through MultiBodyTree + +#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_ +#define MULTI_BODY_REFERENCE_IMPL_HPP_ + +#include "../IDConfig.hpp" +#include "../MultiBodyTree.hpp" + +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 { + ID_DECLARE_ALIGNED_ALLOCATOR(); + // 1 Inertial properties + /// Mass + idScalar m_mass; + /// Mass times center of gravity in body-fixed frame + vec3 m_body_mass_com; + /// Moment of inertia w.r.t. body-fixed frame + mat33 m_body_I_body; + + // 2 dynamic properties + /// Left-hand side of the body equation of motion, translational part + vec3 m_eom_lhs_translational; + /// Left-hand side of the body equation of motion, rotational part + vec3 m_eom_lhs_rotational; + /// Force acting at the joint when the body is cut from its parent; + /// includes impressed joint force in J_JT direction, + /// as well as constraint force, + /// in body-fixed frame + vec3 m_force_at_joint; + /// Moment acting at the joint when the body is cut from its parent; + /// includes impressed joint moment in J_JR direction, and constraint moment + /// in body-fixed frame + vec3 m_moment_at_joint; + /// external (user provided) force acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_force_user; + /// external (user provided) moment acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_moment_user; + // 3 absolute kinematic properties + /// Position of body-fixed frame relative to world frame + /// this is currently only for debugging purposes + vec3 m_body_pos; + /// Absolute velocity of body-fixed frame + vec3 m_body_vel; + /// Absolute acceleration of body-fixed frame + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_acc; + /// Absolute angular velocity + vec3 m_body_ang_vel; + /// Absolute angular acceleration + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_ang_acc; + + // 4 relative kinematic properties. + // these are in the parent body frame + /// Transform from world to body-fixed frame; + /// this is currently only for debugging purposes + mat33 m_body_T_world; + /// Transform from parent to body-fixed frame + mat33 m_body_T_parent; + /// Vector from parent to child frame in parent frame + vec3 m_parent_pos_parent_body; + /// Relative angular velocity + vec3 m_body_ang_vel_rel; + /// Relative linear velocity + vec3 m_parent_vel_rel; + /// Relative angular acceleration + vec3 m_body_ang_acc_rel; + /// Relative linear acceleration + vec3 m_parent_acc_rel; + + // 5 Data describing the joint type and geometry + /// Type of joint + JointType m_joint_type; + /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame + /// Components are in body-fixed frame of the parent + vec3 m_parent_pos_parent_body_ref; + /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame + mat33 m_body_T_parent_ref; + /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. + /// (NOTE: dimensions will have to be dynamic for additional joint types!) + vec3 m_Jac_JR; + /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. + /// (NOTE: dimensions might have to be dynamic for additional joint types!) + vec3 m_Jac_JT; + /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT + vec3 m_parent_Jac_JT; + /// Start of index range for the position degree(s) of freedom describing this body's motion + /// relative to + /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. + int m_q_index; + + // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" + /// mass of the subtree rooted in this body + idScalar m_subtree_mass; + /// center of mass * mass for subtree rooted in this body, in body-fixed frame + 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; +}; + +/// The MBS implements a tree structured multibody system +class MultiBodyTree::MultiBodyImpl { + friend class MultiBodyTree; + +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// constructor + /// @param num_bodies the number of bodies in the system + /// @param num_dofs number of degrees of freedom in the system + MultiBodyImpl(int num_bodies_, int num_dofs_); + + /// \copydoc MultiBodyTree::calculateInverseDynamics + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + ///\copydoc MultiBodyTree::calculateMassMatrix + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + /// generate additional index sets from the parent_index array + /// @return -1 on error, 0 on success + int generateIndexSets(); + /// set gravity acceleration in world frame + /// @param gravity gravity vector in the world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// pretty print tree + void printTree(); + /// print tree data + void printTreeData(); + /// initialize fixed data + void calculateStaticData(); + /// \copydoc MultiBodyTree::getBodyFrame + int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; + /// \copydoc MultiBodyTree::getParentIndex + int getParentIndex(const int body_index, int* m_parent_index); + /// \copydoc MultiBodyTree::getJointType + 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:getDoFOffset + int getDoFOffset(const int body_index, int* q_index) const; + /// \copydoc MultiBodyTree::getBodyOrigin + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// \copydoc MultiBodyTree::getBodyCoM + int getBodyCoM(const int body_index, vec3* world_com) const; + /// \copydoc MultiBodyTree::getBodyTransform + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// \copydoc MultiBodyTree::getBodyAngularVelocity + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocity + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyAngularAcceleration + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearAcceleration + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// \copydoc MultiBodyTree::getUserInt + int getUserInt(const int body_index, int* user_int) const; + /// \copydoc MultiBodyTree::getUserPtr + int getUserPtr(const int body_index, void** user_ptr) const; + /// \copydoc MultiBodyTree::setUserInt + int setUserInt(const int body_index, const int user_int); + /// \copydoc MultiBodyTree::setUserPtr + int setUserPtr(const int body_index, void* const user_ptr); + ///\copydoc MultiBodytTree::setBodyMass + int setBodyMass(const int body_index, const idScalar mass); + ///\copydoc MultiBodytTree::setBodyFirstMassMoment + int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + ///\copydoc MultiBodytTree::setBodySecondMassMoment + int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + ///\copydoc MultiBodytTree::getBodyMass + int getBodyMass(const int body_index, idScalar* mass) const; + ///\copydoc MultiBodytTree::getBodyFirstMassMoment + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + ///\copydoc MultiBodytTree::getBodySecondMassMoment + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments + void clearAllUserForcesAndMoments(); + /// \copydoc MultiBodyTree::addUserForce + int addUserForce(const int body_index, const vec3& body_force); + /// \copydoc MultiBodyTree::addUserMoment + int addUserMoment(const int body_index, const vec3& body_moment); + +private: + // debug function. print tree structure to stdout + void printTree(int index, int indentation); + // get string representation of JointType (for debugging) + const char* jointTypeToString(const JointType& type) const; + // get number of degrees of freedom from joint type + int bodyNumDoFs(const JointType& type) const; + // number of bodies in the system + int m_num_bodies; + // number of degrees of freedom + int m_num_dofs; + // Gravitational acceleration (in world frame) + vec3 m_world_gravity; + // vector of bodies in the system + // body 0 is used as an environment body and is allways fixed. + // The bodies are ordered such that a parent body always has an index + // smaller than its child. + idArray::type m_body_list; + // Parent_index[i] is the index for i's parent body in body_list. + // This fully describes the tree. + idArray::type m_parent_index; + // child_indices[i] contains a vector of indices of + // all children of the i-th body + idArray::type>::type m_child_indices; + // Indices of rotary joints + idArray::type m_body_revolute_list; + // Indices of prismatic joints + idArray::type m_body_prismatic_list; + // Indices of floating joints + idArray::type m_body_floating_list; + // a user-provided integer + idArray::type m_user_int; + // a user-provided pointer + idArray::type m_user_ptr; +}; +} +#endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp new file mode 100644 index 000000000..37063bfd7 --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -0,0 +1,113 @@ +#include "MultiBodyTreeInitCache.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::InitCache::InitCache() { + m_inertias.resize(0); + m_joints.resize(0); + m_num_dofs = 0; + m_root_index=-1; +} + +int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, + const JointType joint_type, + const vec3& parent_r_parent_body_ref, + 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) { + case REVOLUTE: + case PRISMATIC: + m_num_dofs += 1; + break; + case FIXED: + // does not add a degree of freedom + // m_num_dofs+=0; + break; + case FLOATING: + m_num_dofs += 6; + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + if(-1 == parent_index) { + if(m_root_index>=0) { + error_message("trying to add body %d as root, but already added %d as root body\n", + body_index, m_root_index); + return -1; + } + m_root_index=body_index; + } + + JointData joint; + joint.m_child = body_index; + joint.m_parent = parent_index; + joint.m_type = joint_type; + joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; + joint.m_child_T_parent_ref = body_T_parent_ref; + joint.m_child_axis_of_motion = body_axis_of_motion; + + InertiaData body; + body.m_mass = mass; + body.m_body_pos_body_com = body_r_body_com; + body.m_body_I_body = body_I_body; + + m_inertias.push_back(body); + m_joints.push_back(joint); + m_user_int.push_back(user_int); + 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(m_inertias.size())) { + error_message("index out of range\n"); + return -1; + } + + *inertia = m_inertias[index]; + return 0; +} + +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { + if (index < 0 || index > static_cast(m_user_int.size())) { + error_message("index out of range\n"); + return -1; + } + *user_int = m_user_int[index]; + return 0; +} + +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { + if (index < 0 || index > static_cast(m_user_ptr.size())) { + error_message("index out of range\n"); + return -1; + } + *user_ptr = m_user_ptr[index]; + return 0; +} + +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { + if (index < 0 || index > static_cast(m_joints.size())) { + error_message("index out of range\n"); + return -1; + } + *joint = m_joints[index]; + return 0; +} + +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++) { + const JointData& joint = m_joints[j]; + m_parent_index[joint.m_child] = joint.m_parent; + } + + return 0; +} +} diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp new file mode 100644 index 000000000..a46cc16bb --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -0,0 +1,109 @@ +#ifndef MULTIBODYTREEINITCACHE_HPP_ +#define MULTIBODYTREEINITCACHE_HPP_ + +#include "../IDConfig.hpp" +#include "../IDMath.hpp" +#include "../MultiBodyTree.hpp" + +namespace btInverseDynamics { +/// Mass properties of a rigid body +struct InertiaData { + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// mass + idScalar m_mass; + /// vector from body-fixed frame to center of mass, + /// in body-fixed frame, multiplied by the mass + vec3 m_body_pos_body_com; + /// moment of inertia w.r.t. the origin of the body-fixed + /// frame, represented in that frame + mat33 m_body_I_body; +}; + +/// Joint properties +struct JointData { + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// type of joint + JointType m_type; + /// index of parent body + int m_parent; + /// index of child body + int m_child; + /// vector from parent's body-fixed frame to child's body-fixed + /// frame for q=0, written in the parent's body fixed frame + vec3 m_parent_pos_parent_child_ref; + /// Transform matrix converting vectors written in the parent's frame + /// into vectors written in the child's frame for q=0 + /// ie, child_vector = child_T_parent_ref * parent_vector; + mat33 m_child_T_parent_ref; + /// Axis of motion for 1 degree-of-freedom joints, + /// written in the child's frame + /// For revolute joints, the q-value is positive for a positive + /// rotation about this axis. + /// For prismatic joints, the q-value is positive for a positive + /// translation is this direction. + vec3 m_child_axis_of_motion; +}; + +/// Data structure to store data passed by the user. +/// This is used in MultiBodyTree::finalize to build internal data structures. +class MultiBodyTree::InitCache { +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + /// constructor + InitCache(); + ///\copydoc MultiBodyTree::addBody + int addBody(const int body_index, const int parent_index, const JointType joint_type, + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com, + const mat33 &body_I_body, const int user_int, void *user_ptr); + /// build index arrays + /// @return 0 on success, -1 on failure + int buildIndexSets(); + /// @return number of degrees of freedom + int numDoFs() const { return m_num_dofs; } + /// @return number of bodies + int numBodies() const { return m_inertias.size(); } + /// get inertia data for index + /// @param index of the body + /// @param inertia pointer for return data + /// @return 0 on success, -1 on failure + int getInertiaData(const int index, InertiaData *inertia) const; + /// get joint data for index + /// @param index of the body + /// @param joint pointer for return data + /// @return 0 on success, -1 on failure + int getJointData(const int index, JointData *joint) const; + /// get parent index array (paren_index[i] is the index of the parent of i) + /// @param parent_index pointer for return data + void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } + /// get user integer + /// @param index body index + /// @param user_int user integer + /// @return 0 on success, -1 on failure + int getUserInt(const int index, int *user_int) const; + /// get user pointer + /// @param index body index + /// @param user_int user pointer + /// @return 0 on success, -1 on failure + int getUserPtr(const int index, void **user_ptr) const; + +private: + // vector of bodies + idArray::type m_inertias; + // vector of joints + idArray::type m_joints; + // number of mechanical degrees of freedom + int m_num_dofs; + // parent index array + idArray::type m_parent_index; + // user integers + idArray::type m_user_int; + // user pointers + idArray::type m_user_ptr; + // index of root body (or -1 if not set) + int m_root_index; +}; +} +#endif // MULTIBODYTREEINITCACHE_HPP_ diff --git a/src/BulletInverseDynamics/premake4.lua b/src/BulletInverseDynamics/premake4.lua new file mode 100644 index 000000000..774e037b3 --- /dev/null +++ b/src/BulletInverseDynamics/premake4.lua @@ -0,0 +1,12 @@ + project "BulletInverseDynamics" + + kind "StaticLib" + includedirs { + "..", + } + files { + "IDMath.cpp", + "MultiBodyTree.cpp", + "details/MultiBodyTreeInitCache.cpp", + "details/MultiBodyTreeImpl.cpp", + } diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua new file mode 100644 index 000000000..73730b0ab --- /dev/null +++ b/test/InverseDynamics/premake4.lua @@ -0,0 +1,34 @@ + + project "Test_InverseDynamicsKinematics" + + kind "ConsoleApp" + +-- defines { } + + + + includedirs + { + ".", + "../../src", + "../../examples/InverseDynamics", + "../../Extras/InverseDynamics", + "../gtest-1.7.0/include" + + } + + + if os.is("Windows") then + --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012 + defines {"_VARIADIC_MAX=10"} + end + + links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"} + + files { + "test_invdyn_kinematics.cpp", + } + + if os.is("Linux") then + links {"pthread"} + end diff --git a/test/InverseDynamics/test_invdyn_bullet.cpp b/test/InverseDynamics/test_invdyn_bullet.cpp new file mode 100644 index 000000000..ab7578a48 --- /dev/null +++ b/test/InverseDynamics/test_invdyn_bullet.cpp @@ -0,0 +1,115 @@ +/// create a bullet btMultiBody model of a tree structured multibody system, +/// convert that model to a MultiBodyTree model. +/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces +/// - run forward dynamics on (q,u, forces) to get accelerations +/// - compare input accelerations to inverse dynamics to output from forward dynamics + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include <../CommonInterfaces/CommonGUIHelperInterface.h> +#include +#include +#include <../Importers/ImportURDFDemo/BulletUrdfImporter.h> +#include <../Importers/ImportURDFDemo/URDF2Bullet.h> +#include <../Importers/ImportURDFDemo/MyMultiBodyCreator.h> +#include <../Importers/ImportURDFDemo/URDF2Bullet.h> +#include "../../examples/Utils/b3ResourcePath.h" +#include +#include +#include +#include + +using namespace btInverseDynamics; + +DEFINE_bool(verbose, false, "print extra info"); + +static btVector3 gravity(0, 0, -10); +static const bool kBaseFixed = false; +static const char kUrdfFile[] = "r2d2.urdf"; + +/// this test loads the a urdf model with fixed, floating, prismatic and rotational joints, +/// converts in to an inverse dynamics model and compares forward to inverse dynamics for +/// random input +TEST(InvDynCompare, bulletUrdfR2D2) { + MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed); + + char relativeFileName[1024]; + + ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024)); + + mb_load.setFileName(relativeFileName); + mb_load.init(); + + btMultiBodyTreeCreator id_creator; + btMultiBody *btmb = mb_load.getBtMultiBody(); + ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0); + + MultiBodyTree *id_tree = CreateMultiBodyTree(id_creator); + ASSERT_EQ(0x0 != id_tree, true); + + vecx q(id_tree->numDoFs()); + vecx u(id_tree->numDoFs()); + vecx dot_u(id_tree->numDoFs()); + vecx joint_forces(id_tree->numDoFs()); + + const int kNLoops = 10; + double max_pos_error = 0; + double max_acc_error = 0; + + std::default_random_engine generator; + std::uniform_real_distribution distribution(-M_PI, M_PI); + auto rnd = std::bind(distribution, generator); + + for (int loop = 0; loop < kNLoops; loop++) { + for (int i = 0; i < q.size(); i++) { + q(i) = rnd(); + u(i) = rnd(); + dot_u(i) = rnd(); + } + + double pos_error; + double acc_error; + btmb->clearForcesAndTorques(); + id_tree->clearAllUserForcesAndMoments(); + // call inverse dynamics once, to get global position & velocity of root body + // (fixed, so q, u, dot_u arbitrary) + EXPECT_EQ(id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces), 0); + + EXPECT_EQ(compareInverseAndForwardDynamics(q, u, dot_u, gravity, FLAGS_verbose, btmb, id_tree, + &pos_error, &acc_error), + 0); + + if (pos_error > max_pos_error) { + max_pos_error = pos_error; + } + if (acc_error > max_acc_error) { + max_acc_error = acc_error; + } + } + + if (FLAGS_verbose) { + printf("max_pos_error= %e\n", max_pos_error); + printf("max_acc_error= %e\n", max_acc_error); + } + + EXPECT_LT(max_pos_error, std::numeric_limits::epsilon()*1e4); + EXPECT_LT(max_acc_error, std::numeric_limits::epsilon()*1e5); +} + +int main(int argc, char **argv) { + gflags::SetUsageMessage("Usage: invdyn_from_btmultibody -verbose = true/false"); + gflags::ParseCommandLineFlags(&argc, &argv, false); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp new file mode 100644 index 000000000..dd5d443df --- /dev/null +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -0,0 +1,350 @@ +// Test of kinematic consistency: check if finite differences of velocities, accelerations +// match positions + +#include +#include +#include +#include + +#include + +#include "../Extras/InverseDynamics/CoilCreator.hpp" +#include "../Extras/InverseDynamics/DillCreator.hpp" +#include "../Extras/InverseDynamics/SimpleTreeCreator.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" + +using namespace btInverseDynamics; + +const int kLevel = 5; +const int kNumBodies = std::pow(2, kLevel); + +// template function for calculating the norm +template +idScalar calculateNorm(T&); +// only implemented for vec3 +template <> +idScalar calculateNorm(vec3& v) { + return std::sqrt(std::pow(v(0), 2) + std::pow(v(1), 2) + std::pow(v(2), 2)); +} + +// template function to convert a DiffType (finite differences) +// to a ValueType. This is for angular velocity calculations +// via finite differences. +template +DiffType toDiffType(ValueType& fd, ValueType& val); + +// vector case: just return finite difference approximation +template <> +vec3 toDiffType(vec3& fd, vec3& val) { + return fd; +} + +// orientation case: calculate spin tensor and extract angular velocity +template <> +vec3 toDiffType(mat33& fd, mat33& val) { + // spin tensor + mat33 omega_tilde = fd * val.transpose(); + // extract vector from spin tensor + vec3 omega; + omega(0) = 0.5 * (omega_tilde(2, 1) - omega_tilde(1, 2)); + omega(1) = 0.5 * (omega_tilde(0, 2) - omega_tilde(2, 0)); + omega(2) = 0.5 * (omega_tilde(1, 0) - omega_tilde(0, 1)); + return omega; +} + +/// Class for calculating finite difference approximation +/// of time derivatives and comparing it to an analytical solution +/// DiffType and ValueType can be different, to allow comparison +/// of angular velocity vectors and orientations given as transform matrices. +template +class DiffFD { +public: + DiffFD() : m_dt(0.0), m_num_updates(0), m_max_error(0.0), m_max_value(0.0), m_valid_fd(false) {} + + void init(std::string name, idScalar dt) { + m_name = name; + m_dt = dt; + m_num_updates = 0; + m_max_error = 0.0; + m_max_value = 0.0; + m_valid_fd = false; + } + + void update(const ValueType& val, const DiffType& true_diff) { + m_val = val; + if (m_num_updates > 2) { + // 2nd order finite difference approximation for d(value)/dt + ValueType diff_value_fd = (val - m_older_val) / (2.0 * m_dt); + // convert to analytical diff type. This is for angular velocities + m_diff_fd = toDiffType(diff_value_fd, m_old_val); + // now, calculate the error + DiffType error_value_type = m_diff_fd - m_old_true_diff; + idScalar error = calculateNorm(error_value_type); + if (error > m_max_error) { + m_max_error = error; + } + + idScalar value = calculateNorm(m_old_true_diff); + if (value > m_max_value) { + m_max_value = value; + } + + m_valid_fd = true; + } + m_older_val = m_old_val; + m_old_val = m_val; + m_old_true_diff = true_diff; + m_num_updates++; + m_time += m_dt; + } + + void printMaxError() { + printf("max_error: %e dt= %e max_value= %e fraction= %e\n", m_max_error, m_dt, m_max_value, + m_max_value > 0.0 ? m_max_error / m_max_value : 0.0); + } + void printCurrent() { + if (m_valid_fd) { + // note: m_old_true_diff already equals m_true_diff here, so values are not aligned. + // (but error calculation takes this into account) + printf("%s time: %e fd: %e %e %e true: %e %e %e\n", m_name.c_str(), m_time, + m_diff_fd(0), m_diff_fd(1), m_diff_fd(2), m_old_true_diff(0), m_old_true_diff(1), + m_old_true_diff(2)); + } + } + + idScalar getMaxError() const { return m_max_error; } + idScalar getMaxValue() const { return m_max_value; } + +private: + idScalar m_dt; + ValueType m_val; + ValueType m_old_val; + ValueType m_older_val; + DiffType m_old_true_diff; + DiffType m_diff_fd; + int m_num_updates; + idScalar m_max_error; + idScalar m_max_value; + idScalar m_time; + std::string m_name; + bool m_valid_fd; +}; + +template +class VecDiffFD { +public: + VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) { + for (int i = 0; i < m_fd.size(); i++) { + char buf[256]; + snprintf(buf, 256, "%s-%.2d", name.c_str(), i); + m_fd[i].init(buf, dt); + } + } + void update(int i, ValueType& val, DiffType& true_diff) { m_fd[i].update(val, true_diff); } + idScalar getMaxError() const { + idScalar max_error = 0; + for (int i = 0; i < m_fd.size(); i++) { + const idScalar error = m_fd[i].getMaxError(); + if (error > max_error) { + max_error = error; + } + } + return max_error; + } + + void printMaxError() { + printf("%s: total dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError()); + } + + void printCurrent() { + for (int i = 0; i < m_fd.size(); i++) { + m_fd[i].printCurrent(); + } + } + +private: + std::string m_name; + std::vector > m_fd; + const idScalar m_dt; + idScalar m_max_error; +}; + +// calculate maximum difference between finite difference and analytical differentiation +int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar deltaT, + idScalar endTime, idScalar* max_linear_velocity_error, + idScalar* max_angular_velocity_error, + idScalar* max_linear_acceleration_error, + idScalar* max_angular_acceleration_error) { + // setup system + MultiBodyTree* tree = CreateMultiBodyTree(creator); + if (0x0 == tree) { + return -1; + } + // set gravity to zero, so nothing is added to accelerations in forward kinematics + vec3 gravity_zero; + gravity_zero(0) = 0; + gravity_zero(1) = 0; + gravity_zero(2) = 0; + tree->setGravityInWorldFrame(gravity_zero); + // + const idScalar kAmplitude = 1.0; + const idScalar kFrequency = 1.0; + + vecx q(tree->numDoFs()); + vecx dot_q(tree->numDoFs()); + vecx ddot_q(tree->numDoFs()); + vecx joint_forces(tree->numDoFs()); + + VecDiffFD fd_vel("linear-velocity", tree->numBodies(), deltaT); + VecDiffFD fd_acc("linear-acceleration", tree->numBodies(), deltaT); + VecDiffFD fd_omg("angular-velocity", tree->numBodies(), deltaT); + VecDiffFD fd_omgd("angular-acceleration", tree->numBodies(), deltaT); + + for (idScalar t = 0.0; t < endTime; t += deltaT) { + for (int body = 0; body < tree->numBodies(); body++) { + q(body) = kAmplitude * sin(t * 2.0 * M_PI * kFrequency); + dot_q(body) = kAmplitude * 2.0 * M_PI * kFrequency * cos(t * 2.0 * M_PI * kFrequency); + ddot_q(body) = + -kAmplitude * pow(2.0 * M_PI * kFrequency, 2) * sin(t * 2.0 * M_PI * kFrequency); + } + + if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) { + delete tree; + return -1; + } + + // position/velocity + for (int body = 0; body < tree->numBodies(); body++) { + vec3 pos; + vec3 vel; + mat33 world_T_body; + vec3 omega; + vec3 dot_omega; + vec3 acc; + + tree->getBodyOrigin(body, &pos); + tree->getBodyTransform(body, &world_T_body); + tree->getBodyLinearVelocity(body, &vel); + tree->getBodyAngularVelocity(body, &omega); + tree->getBodyLinearAcceleration(body, &acc); + tree->getBodyAngularAcceleration(body, &dot_omega); + + fd_vel.update(body, pos, vel); + fd_omg.update(body, world_T_body, omega); + fd_acc.update(body, vel, acc); + fd_omgd.update(body, omega, dot_omega); + } + } + + *max_linear_velocity_error = fd_vel.getMaxError(); + *max_angular_velocity_error = fd_omg.getMaxError(); + *max_linear_acceleration_error = fd_acc.getMaxError(); + *max_angular_acceleration_error = fd_omgd.getMaxError(); + + delete tree; + return 0; +} + +// first test: absolute difference between numerical and numerial +// differentiation should be small +TEST(InvDynKinematicsDifferentiation, errorAbsolute) { + const idScalar kDeltaT = 1e-7; + const idScalar kDuration = 1e-2; + const idScalar kAcceptableError = 1e-4; + + CoilCreator coil_creator(kNumBodies); + DillCreator dill_creator(kLevel); + SimpleTreeCreator simple_creator(kNumBodies); + + idScalar max_linear_velocity_error; + idScalar max_angular_velocity_error; + idScalar max_linear_acceleration_error; + idScalar max_angular_acceleration_error; + + // test serial chain + calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error, + &max_angular_velocity_error, &max_linear_acceleration_error, + &max_angular_acceleration_error); + + EXPECT_LT(max_linear_velocity_error, kAcceptableError); + EXPECT_LT(max_angular_velocity_error, kAcceptableError); + EXPECT_LT(max_linear_acceleration_error, kAcceptableError); + EXPECT_LT(max_angular_acceleration_error, kAcceptableError); + + // test branched tree + calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error, + &max_angular_velocity_error, &max_linear_acceleration_error, + &max_angular_acceleration_error); + + EXPECT_LT(max_linear_velocity_error, kAcceptableError); + EXPECT_LT(max_angular_velocity_error, kAcceptableError); + EXPECT_LT(max_linear_acceleration_error, kAcceptableError); + EXPECT_LT(max_angular_acceleration_error, kAcceptableError); + + // test system with different joint types + calculateDifferentiationError(simple_creator, kDeltaT, kDuration, &max_linear_velocity_error, + &max_angular_velocity_error, &max_linear_acceleration_error, + &max_angular_acceleration_error); + + EXPECT_LT(max_linear_velocity_error, kAcceptableError); + EXPECT_LT(max_angular_velocity_error, kAcceptableError); + EXPECT_LT(max_linear_acceleration_error, kAcceptableError); + EXPECT_LT(max_angular_acceleration_error, kAcceptableError); +} + +// second test: check if the change in the differentiation error +// is consitent with the second order approximation, ie, error ~ O(dt^2) +TEST(InvDynKinematicsDifferentiation, errorOrder) { + const idScalar kDeltaTs[2] = {1e-4, 1e-5}; + const idScalar kDuration = 1e-2; + + CoilCreator coil_creator(kNumBodies); + // DillCreator dill_creator(kLevel); + // SimpleTreeCreator simple_creator(kNumBodies); + + idScalar max_linear_velocity_error[2]; + idScalar max_angular_velocity_error[2]; + idScalar max_linear_acceleration_error[2]; + idScalar max_angular_acceleration_error[2]; + + // test serial chain + calculateDifferentiationError(coil_creator, kDeltaTs[0], kDuration, + &max_linear_velocity_error[0], &max_angular_velocity_error[0], + &max_linear_acceleration_error[0], + &max_angular_acceleration_error[0]); + + calculateDifferentiationError(coil_creator, kDeltaTs[1], kDuration, + &max_linear_velocity_error[1], &max_angular_velocity_error[1], + &max_linear_acceleration_error[1], + &max_angular_acceleration_error[1]); + + const idScalar expected_linear_velocity_error_1 = + max_linear_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + const idScalar expected_angular_velocity_error_1 = + max_angular_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + const idScalar expected_linear_acceleration_error_1 = + max_linear_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + const idScalar expected_angular_acceleration_error_1 = + max_angular_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + + printf("linear vel error: %e %e %e\n", max_linear_velocity_error[1], + expected_linear_velocity_error_1, + max_linear_velocity_error[1] - expected_linear_velocity_error_1); + printf("angular vel error: %e %e %e\n", max_angular_velocity_error[1], + expected_angular_velocity_error_1, + max_angular_velocity_error[1] - expected_angular_velocity_error_1); + printf("linear acc error: %e %e %e\n", max_linear_acceleration_error[1], + expected_linear_acceleration_error_1, + max_linear_acceleration_error[1] - expected_linear_acceleration_error_1); + printf("angular acc error: %e %e %e\n", max_angular_acceleration_error[1], + expected_angular_acceleration_error_1, + max_angular_acceleration_error[1] - expected_angular_acceleration_error_1); +} + +int main(int argc, char** argv) { + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); + + return EXIT_SUCCESS; +}