From 703767c6f3b63e55bc9835b7f3f3041cc65a4242 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 14 May 2018 07:34:49 -0700 Subject: [PATCH] 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. --- src/BulletInverseDynamics/MultiBodyTree.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index c67588d49..0f6668fdb 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -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) {