Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixes bug where user data defined in a URDF link wasn't being applied on

Browse Source 
the correct link.
This commit is contained in:
Tigran Gasparian
2020-05-11 19:27:04 +02:00
parent 004dcc3404
commit faa708db49
1 changed files with 24 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3330,17 +3330,32 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
}
const UrdfModel* urdfModel = u2b.getUrdfModel();
if (urdfModel) {
addUserData(urdfModel->m_userData, bodyUniqueId);
for (int linkIndex = 0; linkIndex < urdfModel->m_links.size(); ++linkIndex) {
const UrdfLink* link = *urdfModel->m_links.getAtIndex(linkIndex);
addUserData(link->m_userData, bodyUniqueId, linkIndex - 1);
for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) {
addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, linkIndex - 1, visualShapeIndex);
// 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<btHashString, int> 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);
}
}
const UrdfModel* urdfModel = u2b.getUrdfModel();
if (urdfModel) {
addUserData(urdfModel->m_userData, bodyUniqueId);
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);
}
}
}
}
}
b3Notification notification;
notification.m_notificationType = BODY_ADDED;