From d26dbef2dc5a88fd2642157a165ed7735470aa5f Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Wed, 23 Feb 2022 16:45:52 +0100 Subject: Also adds user data specified in URDF files to rigid bodies (previously, only multi bodies were added correctly). --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 52 +++++++++++++--------- 1 file changed, 32 insertions(+), 20 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 357101303..ae3142e15 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3433,34 +3433,46 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, } } - // Because the link order between UrdfModel and MultiBody may be different, - // create a mapping from link name to link index in order to apply the user - // data to the correct link in the MultiBody. - btHashMap linkNameToIndexMap; - if (bodyHandle->m_multiBody) - { - btMultiBody* mb = bodyHandle->m_multiBody; - linkNameToIndexMap.insert(mb->getBaseName(), -1); - for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex) - { - linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex); - } - } - + // Add user data specified in URDF to the added body. const UrdfModel* urdfModel = u2b.getUrdfModel(); if (urdfModel) { addUserData(urdfModel->m_userData, bodyUniqueId); - for (int i = 0; i < urdfModel->m_links.size(); ++i) + if (bodyHandle->m_multiBody) + { + btMultiBody* mb = bodyHandle->m_multiBody; + // Because the link order between UrdfModel and MultiBody may be different, + // create a mapping from link name to link index in order to apply the user + // data to the correct link in the MultiBody. + btHashMap linkNameToIndexMap; + linkNameToIndexMap.insert(mb->getBaseName(), -1); + for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex) + { + linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex); + } + for (int i = 0; i < urdfModel->m_links.size(); ++i) + { + const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); + int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); + if (linkIndex) + { + addUserData(link->m_userData, bodyUniqueId, *linkIndex); + for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) + { + addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); + } + } + } + } + else if (bodyHandle->m_rigidBody) { - const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); - int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); - if (linkIndex) + for (int i = 0; i < urdfModel->m_links.size(); ++i) { - addUserData(link->m_userData, bodyUniqueId, *linkIndex); + const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); + addUserData(link->m_userData, bodyUniqueId, -1); for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) { - addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); + addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, -1, visualShapeIndex); } } } -- cgit v1.2.1