add real-time safe Inverse Dynamics library+test+utils

an example for the example browser will follow. thanks to Thomas Buschmann
2015-11-17 08:27:38 -08:00
parent 7d9365319c
commit aa4d119f98
41 changed files with 5233 additions and 0 deletions
--- a/Extras/InverseDynamics/CoilCreator.cpp
+++ b/Extras/InverseDynamics/CoilCreator.cpp
@@ -0,0 +1,67 @@
+#include <cmath>
+
+#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;
+}
+}