diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp index 22869b7ff279acdfc121fbadba0cc1f3f050a514..025904d0df8f18d24a4cd0f13f03d3670547fbf1 100644 --- a/VirtualRobot/RobotFactory.cpp +++ b/VirtualRobot/RobotFactory.cpp @@ -1184,32 +1184,47 @@ namespace VirtualRobot const Eigen::Matrix4f globalPoseInv = simox::math::inverted_pose(n.node_cloned->getGlobalPose()); if (!visus.empty()) { - if (const auto visu = n.node->getVisualization()) + if (visus.size() == 1) { - visus.insert(visus.begin(), visu->clone()); + n.node_cloned->setVisualization(visus.front()->clone()); } - auto v = vf->createUnitedVisualization(visus); - if (n.parentNode_cloned) + else { - vf->applyDisplacement(v, globalPoseInv); + if (const auto visu = n.node->getVisualization()) + { + visus.insert(visus.begin(), visu->clone()); + } + auto v = vf->createUnitedVisualization(visus); + if (n.parentNode_cloned) + { + v->setLocalPose(globalPoseInv); + } + n.node_cloned->setVisualization(v); } - n.node_cloned->setVisualization(v); } if (!colVisus.empty()) { - if (const auto colModel = n.node->getCollisionModel()) + if (colVisus.size() == 1) { - if (const auto vis = colModel->getVisualization()) - { - colVisus.insert(colVisus.begin(), vis->clone()); - } + n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisus.front(), n.node->getName())); } - auto colVisu = vf->createUnitedVisualization(colVisus); - if (n.parentNode_cloned) + else { - vf->applyDisplacement(colVisu, globalPoseInv); + if (const auto colModel = n.node->getCollisionModel()) + { + if (const auto vis = colModel->getVisualization()) + { + colVisus.insert(colVisus.begin(), vis->clone()); + } + } + auto colVisu = vf->createUnitedVisualization(colVisus); + if (n.parentNode_cloned) + { + colVisu->setLocalPose(globalPoseInv); + } + n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName())); } - n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName())); + } for (const auto& childNode : n.childNodes) diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp index 7a4e74191658a561faadbe05d921508a8340f759..b41899e0af4952759437b26221807efda5acc983 100644 --- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp +++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp @@ -4076,8 +4076,6 @@ namespace VirtualRobot { SoSeparator* ss = new SoSeparator(); - SoUnits* u = new SoUnits(); - u->units = SoUnits::MILLIMETERS; const Eigen::Isometry3f localPose(visualizations[i]->getLocalPose()); const Eigen::Vector3f& translation = localPose.translation(); @@ -4087,7 +4085,6 @@ namespace VirtualRobot const Eigen::Quaternionf q = simox::math::mat3f_to_quat(localPose.linear()); t->rotation.setValue(q.x(), q.y(), q.z(), q.w()); - ss->addChild(u); ss->addChild(t); ss->addChild(n->copy(FALSE));