From cb55cd3cb61357fc0c9b37f68e744b8fac10570c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 1 Aug 2023 12:08:00 +0200
Subject: [PATCH] Fix using cloned nodes instead of original nodes to get the
correct local trafo, refactoring
---
VirtualRobot/RobotFactory.cpp | 191 +++++++++++++++++++---------------
1 file changed, 107 insertions(+), 84 deletions(-)
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index 1b1ec5265..dbb79067f 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -1254,146 +1254,169 @@ namespace VirtualRobot
}
VirtualRobot::RobotPtr
- RobotFactory::createFlattenedModel(Robot &robot)
+ 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());
+ RobotPtr flattenedRobot =
+ std::make_shared<LocalRobot>("Flattened_" + robot.getName(), robot.getType());
struct NodeToClone
{
- RobotNodePtr original;
- RobotNodePtr parent;
+ RobotNodePtr nodeInOriginalRobot;
+ RobotNodePtr parentInFlattenedRobot;
};
- std::queue<NodeToClone> nodes({{robot.getRootNode(), nullptr}});
-
- while (!nodes.empty())
- {
- NodeToClone current = nodes.front();
- nodes.pop();
+ auto isBody = [](RobotNodePtr const& node) -> bool
+ { return node->getVisualization() != nullptr and node->getChildren<RobotNode>().empty(); };
- RobotNodePtr cloned = current.original->clone(flattenedModel, false, current.parent);
+ auto isLink = [](RobotNodePtr const& node) -> bool {
+ return node->getVisualization() == nullptr and
+ not node->getChildren<RobotNode>().empty();
+ };
- if (current.parent == nullptr)
+ auto pushAllChildren = [](RobotNodePtr const& original,
+ RobotNodePtr const& cloned,
+ std::queue<NodeToClone>& nodes)
+ {
+ for (const auto& child : original->getChildren())
{
- flattenedModel->setRootNode(cloned);
+ auto const& childNode = std::dynamic_pointer_cast<RobotNode>(child);
+ if (childNode == nullptr)
+ {
+ continue;
+ }
+
+ nodes.push({childNode, cloned});
}
+ };
- auto isBody = [](RobotNodePtr const& node) -> bool
- {
- return node->getVisualization() != nullptr and node->getChildren<RobotNode>().empty();
- };
+ struct BodyAndLink
+ {
+ RobotNodePtr body;
+ RobotNodePtr link;
+ std::vector<RobotNodePtr> others;
+ };
- auto isLink = [](RobotNodePtr const& node) -> bool
- {
- return node->getVisualization() == nullptr and not node->getChildren().empty();
- };
+ auto findBodyAndLink = [isBody,
+ isLink](RobotNodePtr const& jointNode) -> std::optional<BodyAndLink>
+ {
+ std::optional<RobotNodePtr> body;
+ std::optional<RobotNodePtr> link;
+ std::vector<RobotNodePtr> others;
- auto pushAllChildren = [&nodes](RobotNodePtr const& original, RobotNodePtr const& cloned)
+ for (const auto& child : jointNode->getChildren())
{
- for (auto const& child : original->getChildren())
+ auto const& childNode = std::dynamic_pointer_cast<RobotNode>(child);
+ if (childNode == nullptr)
{
- auto const& childNode = std::dynamic_pointer_cast<RobotNode>(child);
- if (childNode == nullptr)
- {
- continue;
- }
-
- nodes.push({childNode, cloned});
+ continue;
}
- };
-
- 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())
+ if (isBody(childNode))
{
- auto const& childNode = std::dynamic_pointer_cast<RobotNode>(child);
- if (childNode == nullptr)
+ if (body.has_value())
{
- continue;
+ return std::nullopt;
}
- if (isBody(childNode))
+ body = childNode;
+ }
+ else if (isLink(childNode))
+ {
+ if (link.has_value())
{
- if (body.has_value())
- {
- return std::nullopt;
- }
-
- body = childNode;
+ return std::nullopt;
}
- else if (isLink(childNode))
- {
- if (link.has_value())
- {
- return std::nullopt;
- }
- link = childNode;
- }
- else
- {
- others.emplace_back(childNode);
- }
+ link = childNode;
}
-
- if (body.has_value() and link.has_value())
+ else
{
- return std::make_tuple(body.value(), link.value(), others);
+ others.emplace_back(childNode);
}
+ }
- return std::nullopt;
- };
-
- auto performFlattenedAttach = [&flattenedModel, pushAllChildren](RobotNodePtr const& clonedJoint, RobotNodePtr const& originalBody, RobotNodePtr const& originalLink)
+ if (body.has_value() and link.has_value())
{
- RobotNodePtr clonedBody = originalBody->clone(flattenedModel, false, clonedJoint);
- RobotNodePtr clonedLink = originalLink->clone(flattenedModel, false, clonedBody);
+ return BodyAndLink{.body = body.value(), .link = link.value(), .others = others};
+ }
+
+ return std::nullopt;
+ };
+
+ auto performFlattenedAttach =
+ [&flattenedRobot, pushAllChildren](RobotNodePtr const& clonedJoint,
+ RobotNodePtr const& originalBody,
+ RobotNodePtr const& originalLink,
+ std::queue<NodeToClone>& nodesToClone)
+ {
+ const bool cloneChildren = false;
+ RobotNodePtr clonedBody =
+ originalBody->clone(flattenedRobot, cloneChildren, clonedJoint);
+ RobotNodePtr clonedLink =
+ originalLink->clone(flattenedRobot, cloneChildren, clonedBody);
+
+ Eigen::Matrix4f global_T_body = clonedBody->getGlobalPose();
+ Eigen::Matrix4f global_T_link = clonedLink->getGlobalPose();
+
+ Eigen::Matrix4f body_T_link = simox::math::inverted_pose(global_T_body) * global_T_link;
+
+ clonedLink->setLocalTransformation(body_T_link);
+ clonedLink->updateTransformationMatrices();
+
+ pushAllChildren(originalLink, clonedLink, nodesToClone);
+ };
+
+ std::queue<NodeToClone> nodes({{robot.getRootNode(), nullptr}});
+ while (not nodes.empty())
+ {
+ NodeToClone current = nodes.front();
+ nodes.pop();
- Eigen::Matrix4f localLinkTransform = simox::math::inverted_pose(clonedBody->getGlobalPose()) * clonedLink->getGlobalPose();
- clonedLink->setLocalTransformation(localLinkTransform);
- clonedLink->updateTransformationMatrices();
+ const bool cloneChildren = false;
+ RobotNodePtr cloned = current.nodeInOriginalRobot->clone(
+ flattenedRobot, cloneChildren, current.parentInFlattenedRobot);
- pushAllChildren(originalLink, clonedLink);
- };
+ if (current.parentInFlattenedRobot == nullptr)
+ {
+ flattenedRobot->setRootNode(cloned);
+ }
- if (current.original->isJoint())
+ if (current.nodeInOriginalRobot->isJoint())
{
- auto specials = findSpecialChildren(current.original);
- if (specials.has_value())
+ std::optional<BodyAndLink> bodyAndLink =
+ findBodyAndLink(current.nodeInOriginalRobot);
+ if (bodyAndLink.has_value())
{
- auto [body, link, others] = specials.value();
+ auto [body, link, others] = bodyAndLink.value();
- performFlattenedAttach(cloned, body, link);
+ performFlattenedAttach(cloned, body, link, nodes);
- for (auto const& other : others)
+ for (const auto& other : others)
{
nodes.push({other, cloned});
}
}
else
{
- pushAllChildren(current.original, cloned);
+ pushAllChildren(current.nodeInOriginalRobot, cloned, nodes);
}
}
else
{
- pushAllChildren(current.original, cloned);
+ pushAllChildren(current.nodeInOriginalRobot, cloned, nodes);
}
}
robot.setGlobalPose(globalPose);
- flattenedModel->setGlobalPose(globalPose);
+ flattenedRobot->setGlobalPose(globalPose);
- return flattenedModel;
+ return flattenedRobot;
}
+
void RobotFactory::cloneRecursiveUnite(RobotPtr robot, RobotNodePtr currentNode, RobotNodePtr currentNodeClone, std::vector<std::string> uniteWithAllChildren)
{
std::vector<SceneObjectPtr> c = currentNode->getChildren();
--
GitLab