diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index d70e28220..02dcf3819 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -202,8 +202,16 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; break; case btMultibodyLink::eSpherical: - bt_id_error_message("spherical joints not implemented\n"); - return -1; + + link.joint_type = SPHERICAL; + 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]; + // random unit vector + link.body_axis_of_motion(0) = 0; + link.body_axis_of_motion(1) = 0; + link.body_axis_of_motion(2) = 1; + break; case btMultibodyLink::ePlanar: bt_id_error_message("planar joints not implemented\n"); return -1; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index db69fa956..0e4593f53 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9756,7 +9756,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff); } - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId; for (int i = 0; i < numDofs; i++) { serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i]; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index e42055f12..be4c5d638 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -134,6 +134,15 @@ public: return m_baseCollider; } + const btMultiBodyLinkCollider *getLinkCollider(int index) const + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } + btMultiBodyLinkCollider *getLinkCollider(int index) { if (index >= 0 && index < getNumLinks()) diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index dadf0bab0..f150b5ae4 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -281,6 +281,8 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ break; case FLOATING: break; + case SPHERICAL: + break; default: bt_id_error_message("unknown joint type %d\n", joint_type); return -1; @@ -437,6 +439,16 @@ int MultiBodyTree::finalize() rigid_body.m_Jac_JT(1) = 0.0; rigid_body.m_Jac_JT(2) = 0.0; break; + case SPHERICAL: + // 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; case FLOATING: // NOTE/TODO: this is not really correct. // the Jacobians should be 3x3 matrices here ! diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index cf41af5b2..a718db051 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -28,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind // does not add a degree of freedom // m_num_dofs+=0; break; + case SPHERICAL: + m_num_dofs += 3; + break; case FLOATING: m_num_dofs += 6; break;