1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
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<int>::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<InertiaData>::type m_inertias;
// vector of joints
idArray<JointData>::type m_joints;
// number of mechanical degrees of freedom
int m_num_dofs;
// parent index array
idArray<int>::type m_parent_index;
// user integers
idArray<int>::type m_user_int;
// user pointers
idArray<void *>::type m_user_ptr;
// index of root body (or -1 if not set)
int m_root_index;
};
}
#endif // MULTIBODYTREEINITCACHE_HPP_
|