BulletInverseDynamics: Fix propagation of user_int and user_ptr when creating MultiBodyTree.

The user_ptr and user_int arguments passed to addBody were stored in the cache, but not propagated to the MultiBodyTree when finalizing it.

Thanks to Thomas Buschmann for the fix.
This commit is contained in:
Erwin Coumans 2018-05-14 07:34:49 -07:00
parent e5a9b42f9a
commit 703767c6f3

View File

@ -331,6 +331,22 @@ int MultiBodyTree::finalize() {
rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
rigid_body.m_joint_type = joint.m_type;
int user_int;
if (-1 == m_init_cache->getUserInt(index, &user_int)) {
return -1;
}
if (-1 == m_impl->setUserInt(index, user_int)) {
return -1;
}
void* user_ptr;
if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) {
return -1;
}
if (-1 == m_impl->setUserPtr(index, user_ptr)) {
return -1;
}
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
// matrices.
switch (rigid_body.m_joint_type) {