diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp index 025904d0df8f18d24a4cd0f13f03d3670547fbf1..1b1ec5265546b99f9ff7fa7e703a68a49f416adb 100644 --- a/VirtualRobot/RobotFactory.cpp +++ b/VirtualRobot/RobotFactory.cpp @@ -1253,6 +1253,147 @@ namespace VirtualRobot return reducedModel; } + VirtualRobot::RobotPtr + RobotFactory::createFlattenedModel(Robot &robot) + { + const Eigen::Matrix4f globalPose = robot.getGlobalPose(); + robot.setGlobalPose(Eigen::Matrix4f::Identity()); + + RobotPtr flattenedModel = std::make_shared<LocalRobot>("Flattened_" + robot.getName(), robot.getType()); + + struct NodeToClone + { + RobotNodePtr original; + RobotNodePtr parent; + }; + + std::queue<NodeToClone> nodes({{robot.getRootNode(), nullptr}}); + + while (!nodes.empty()) + { + NodeToClone current = nodes.front(); + nodes.pop(); + + RobotNodePtr cloned = current.original->clone(flattenedModel, false, current.parent); + + if (current.parent == nullptr) + { + flattenedModel->setRootNode(cloned); + } + + auto isBody = [](RobotNodePtr const& node) -> bool + { + return node->getVisualization() != nullptr and node->getChildren<RobotNode>().empty(); + }; + + auto isLink = [](RobotNodePtr const& node) -> bool + { + return node->getVisualization() == nullptr and not node->getChildren().empty(); + }; + + auto pushAllChildren = [&nodes](RobotNodePtr const& original, RobotNodePtr const& cloned) + { + for (auto const& child : original->getChildren()) + { + auto const& childNode = std::dynamic_pointer_cast<RobotNode>(child); + if (childNode == nullptr) + { + continue; + } + + nodes.push({childNode, cloned}); + } + }; + + auto findSpecialChildren = [isBody, isLink](RobotNodePtr const& node) -> std::optional<std::tuple<RobotNodePtr, RobotNodePtr, std::vector<RobotNodePtr>>> + { + std::optional<RobotNodePtr> body; + std::optional<RobotNodePtr> link; + std::vector<RobotNodePtr> others; + + for (auto const& child : node->getChildren()) + { + auto const& childNode = std::dynamic_pointer_cast<RobotNode>(child); + if (childNode == nullptr) + { + continue; + } + + if (isBody(childNode)) + { + if (body.has_value()) + { + return std::nullopt; + } + + body = childNode; + } + else if (isLink(childNode)) + { + if (link.has_value()) + { + return std::nullopt; + } + + link = childNode; + } + else + { + others.emplace_back(childNode); + } + } + + if (body.has_value() and link.has_value()) + { + return std::make_tuple(body.value(), link.value(), others); + } + + return std::nullopt; + }; + + auto performFlattenedAttach = [&flattenedModel, pushAllChildren](RobotNodePtr const& clonedJoint, RobotNodePtr const& originalBody, RobotNodePtr const& originalLink) + { + RobotNodePtr clonedBody = originalBody->clone(flattenedModel, false, clonedJoint); + RobotNodePtr clonedLink = originalLink->clone(flattenedModel, false, clonedBody); + + Eigen::Matrix4f localLinkTransform = simox::math::inverted_pose(clonedBody->getGlobalPose()) * clonedLink->getGlobalPose(); + clonedLink->setLocalTransformation(localLinkTransform); + clonedLink->updateTransformationMatrices(); + + pushAllChildren(originalLink, clonedLink); + }; + + if (current.original->isJoint()) + { + auto specials = findSpecialChildren(current.original); + if (specials.has_value()) + { + auto [body, link, others] = specials.value(); + + performFlattenedAttach(cloned, body, link); + + for (auto const& other : others) + { + nodes.push({other, cloned}); + } + } + else + { + pushAllChildren(current.original, cloned); + } + } + else + { + pushAllChildren(current.original, cloned); + } + } + + robot.setGlobalPose(globalPose); + flattenedModel->setGlobalPose(globalPose); + + return flattenedModel; + } + void RobotFactory::cloneRecursiveUnite(RobotPtr robot, RobotNodePtr currentNode, RobotNodePtr currentNodeClone, std::vector<std::string> uniteWithAllChildren) { std::vector<SceneObjectPtr> c = currentNode->getChildren(); diff --git a/VirtualRobot/RobotFactory.h b/VirtualRobot/RobotFactory.h index e73cbe19fe49bbe0680d8ff24d5f820fb391fd69..f86172d50112e1d8df9bd5680b3838c60c2a94f6 100644 --- a/VirtualRobot/RobotFactory.h +++ b/VirtualRobot/RobotFactory.h @@ -118,6 +118,15 @@ namespace VirtualRobot createReducedModel(Robot &robot, const std::vector<std::string>& actuatedJointNames, const std::vector<std::string>& otherNodeNames = std::vector<std::string>()); + /** + * Creates a cloned robot model that flattens the robot structure. + * This means that the pattern <joint> -> [<body>, <link>] is transformed to <joint> -> <body> -> <link> which is required for bullet simulation. + * @param robot The robot to clone. + * @return The created robot. + */ + static VirtualRobot::RobotPtr + createFlattenedModel(Robot &robot); + /*! Creates a clone with changed structure, so that the given robot node is the new root of the resulting kinematic tree. */ diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp index 8865b4f574b8e7d66d91bae3af321dccd4319cd5..9fa39d7e119083132ebad12023bbd6c6876daddc 100644 --- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp +++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp @@ -789,6 +789,7 @@ void showRobotWindow::loadRobot() } robot = importer->loadFromFile(m_sRobotFilename, RobotIO::eFull); + robot = RobotFactory::createFlattenedModel(*robot); } catch (VirtualRobotException& e) {