From daa9b7298dc9e863aaf5974927ba5386bb1f77df Mon Sep 17 00:00:00 2001 From: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Date: Tue, 24 Oct 2023 21:35:34 -0500 Subject: [PATCH 1/2] Fixed parenting of SDF model and link entity transforms The pose information from the SDF is a value relative to it's attached frame. The `GetLocalTransfromURDF` function was misnamed and returning the a local transform for any SDF link supplied to the function. Also the `PrefabMakerUtils::SetEntityParent` function which is used to attach a child entity to a parent entity, preserves the world transform of the child entity. Therefore any pose information on child entities such as nested models or links on a model would be set in world space and not in local space. Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- .../RobotImporter/URDF/PrefabMakerUtils.cpp | 25 ++++++++++++++++--- .../RobotImporter/URDF/PrefabMakerUtils.h | 9 +++++++ .../RobotImporter/URDF/URDFPrefabMaker.cpp | 12 ++++++--- .../Utils/RobotImporterUtils.cpp | 2 +- .../RobotImporter/Utils/RobotImporterUtils.h | 10 ++++---- Gems/ROS2/Code/Tests/UrdfParserTest.cpp | 6 ++--- 6 files changed, 48 insertions(+), 16 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp index 7c0e0774e..4ee0b8fc1 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp @@ -77,7 +77,7 @@ namespace ROS2::PrefabMakerUtils } AZ_Trace("CreateEntity", "Processing entity id: %s with name: %s\n", entityId.ToString().c_str(), name.c_str()); - + // If the parent is invalid, parent to the container of the currently focused prefab if one exists. if (!parentEntityId.IsValid()) { @@ -95,7 +95,7 @@ namespace ROS2::PrefabMakerUtils return entityId; } - void SetEntityParent(AZ::EntityId entityId, AZ::EntityId parentEntityId) + static void SetEntityParentInternal(AZ::EntityId entityId, AZ::EntityId parentEntityId, bool useLocalTransform) { auto* entity = AzToolsFramework::GetEntityById(entityId); AZ_Assert(entity, "Unknown entity %s", entityId.ToString().c_str()); @@ -105,10 +105,29 @@ namespace ROS2::PrefabMakerUtils if (auto* transformComponent = entity->FindComponent(); transformComponent) { - transformComponent->SetParent(parentEntityId); + if (!useLocalTransform) + { + transformComponent->SetParent(parentEntityId); + } + else + { + transformComponent->SetParentRelative(parentEntityId); + } } } + void SetEntityParent(AZ::EntityId entityId, AZ::EntityId parentEntityId) + { + constexpr bool useLocalTransform = false; + return SetEntityParentInternal(entityId, parentEntityId, useLocalTransform); + } + + void SetEntityParentRelative(AZ::EntityId entityId, AZ::EntityId parentEntityId) + { + constexpr bool useLocalTransform = true; + return SetEntityParentInternal(entityId, parentEntityId, useLocalTransform); + } + void AddRequiredComponentsToEntity(AZ::EntityId entityId) { AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h index 553dffc90..3a1bea87c 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h @@ -42,10 +42,19 @@ namespace ROS2::PrefabMakerUtils AzToolsFramework::Prefab::PrefabEntityResult CreateEntity(AZ::EntityId parentEntityId, const AZStd::string& name); //! Set the parent entity for an entity. The entity getting parent is expected to be inactive. + //! NOTE: This uses the world transform of the entity when updating the transform + //! The entity itself will world location will not change //! @param entityId the id for entity that needs a parent. //! @param parentEntityId the id for the parent entity. void SetEntityParent(AZ::EntityId entityId, AZ::EntityId parentEntityId); + //! Set the parent entity for an entity. The entity getting parent is expected to be inactive. + //! NOTE: This uses the local transform of the entity when updating the transform + //! and therefore allows the entity to relocate based on the parent world transform + //! @param entityId the id for entity that needs a parent. + //! @param parentEntityId the id for the parent entity. + void SetEntityParentRelative(AZ::EntityId entityId, AZ::EntityId parentEntityId); + //! Create an entity name from arguments. //! @param rootName root of entity's name. //! @param type type of entity, depending on corresponding SDF tag. For example, "visual". diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index 4f6099b12..d875a4dc9 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -228,7 +228,9 @@ namespace ROS2 // set the current model transform component parent to the parent model if (parentModelEntityId.IsValid() && modelEntityId.IsValid()) { - PrefabMakerUtils::SetEntityParent(modelEntityId, parentModelEntityId); + // The model entity local transform should be used + // to allow it to move, rotate, scale relative to the parent + PrefabMakerUtils::SetEntityParentRelative(modelEntityId, parentModelEntityId); } } @@ -277,7 +279,7 @@ namespace ROS2 { AZ::EntityId createdEntityId = createLinkEntityResult.GetValue(); std::string linkName = linkPtr->Name(); - AZ::Transform tf = Utils::GetWorldTransformURDF(linkPtr); + AZ::Transform tf = Utils::GetLocalTransformURDF(linkPtr); auto* entity = AzToolsFramework::GetEntityById(createdEntityId); if (entity) { @@ -296,7 +298,7 @@ namespace ROS2 tf.GetRotation().GetY(), tf.GetRotation().GetZ(), tf.GetRotation().GetW()); - transformInterface->SetWorldTM(tf); + transformInterface->SetLocalTM(tf); } else { @@ -382,7 +384,9 @@ namespace ROS2 linkPrefabResult.GetValue().ToString().c_str(), parentEntityIter->second.GetValue().ToString().c_str()); AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s setting parent to %s\n", linkName.c_str(), parentName.c_str()); - PrefabMakerUtils::SetEntityParent(linkPrefabResult.GetValue(), parentEntityIter->second.GetValue()); + // As the link transforms are relative, use SetEntityParentRelative to make sure the link are + // translated, scaled and rotated to correct world location + PrefabMakerUtils::SetEntityParentRelative(linkPrefabResult.GetValue(), parentEntityIter->second.GetValue()); } // Iterate over all the joints and locate the entity associated with the link diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index c810c458b..05d66ff7f 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -80,7 +80,7 @@ namespace ROS2::Utils return isWheel; } - AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t) + AZ::Transform GetLocalTransformURDF(const sdf::Link* link, AZ::Transform t) { // Determine if the pose is relative to another link // See doxygen at diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h index 08e755b91..08ba42b1e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h @@ -38,11 +38,11 @@ namespace ROS2::Utils //! @return true if the link is likely a wheel link. bool IsWheelURDFHeuristics(const sdf::Model& model, const sdf::Link* link); - //! The recursive function for the given link goes through URDF and finds world-to-entity transformation for us. - //! @param link pointer to URDF/SDF link that root of robot description - //! @param t initial transform, should be identity for non-recursive call. - //! @returns root to entity transform - AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t = AZ::Transform::Identity()); + //! Returns an AZ::Transform converted from the link pose defined relative to another frame. + //! @param link pointer to URDF/SDF link + //! @param t initial transform, multiplied against link transform + //! @returns Transform of link + AZ::Transform GetLocalTransformURDF(const sdf::Link* link, AZ::Transform t = AZ::Transform::Identity()); //! Type Alias representing a "stack" of Model object that were visited on the way to the current Link/Joint Visitor Callback using ModelStack = AZStd::deque>; diff --git a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp index 65136e524..4cee0dbbf 100644 --- a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp @@ -791,17 +791,17 @@ namespace UnitTest const AZ::Vector3 expected_translation_link2{ -1.2000000476837158, 2.0784599781036377, 0.0 }; const AZ::Vector3 expected_translation_link3{ -2.4000000953674316, 0.0, 0.0 }; - const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(base_link_ptr); + const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetLocalTransformURDF(base_link_ptr); EXPECT_NEAR(expected_translation_link1.GetX(), transform_from_urdf_link1.GetTranslation().GetX(), 1e-5); EXPECT_NEAR(expected_translation_link1.GetY(), transform_from_urdf_link1.GetTranslation().GetY(), 1e-5); EXPECT_NEAR(expected_translation_link1.GetZ(), transform_from_urdf_link1.GetTranslation().GetZ(), 1e-5); - const AZ::Transform transform_from_urdf_link2 = ROS2::Utils::GetWorldTransformURDF(link2_ptr); + const AZ::Transform transform_from_urdf_link2 = ROS2::Utils::GetLocalTransformURDF(link2_ptr); EXPECT_NEAR(expected_translation_link2.GetX(), transform_from_urdf_link2.GetTranslation().GetX(), 1e-5); EXPECT_NEAR(expected_translation_link2.GetY(), transform_from_urdf_link2.GetTranslation().GetY(), 1e-5); EXPECT_NEAR(expected_translation_link2.GetZ(), transform_from_urdf_link2.GetTranslation().GetZ(), 1e-5); - const AZ::Transform transform_from_urdf_link3 = ROS2::Utils::GetWorldTransformURDF(link3_ptr); + const AZ::Transform transform_from_urdf_link3 = ROS2::Utils::GetLocalTransformURDF(link3_ptr); EXPECT_NEAR(expected_translation_link3.GetX(), transform_from_urdf_link3.GetTranslation().GetX(), 1e-5); EXPECT_NEAR(expected_translation_link3.GetY(), transform_from_urdf_link3.GetTranslation().GetY(), 1e-5); EXPECT_NEAR(expected_translation_link3.GetZ(), transform_from_urdf_link3.GetTranslation().GetZ(), 1e-5); From 65fbdddb886c7b28193c849ecdc9b8af1ba9a2d3 Mon Sep 17 00:00:00 2001 From: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Date: Wed, 25 Oct 2023 11:24:47 -0500 Subject: [PATCH 2/2] Fixed the setup of the parent child entity hierarchy between SDF links The parent child hierarchy is established through an SDF joint element which references a child and a parent link. The hieararchy itself shouldn't be used to adjust the world transform between the two links, only to have the child link entity to be under parent link entity for organization purposes. Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- .../Source/RobotImporter/URDF/PrefabMakerUtils.cpp | 10 ++++++---- .../Code/Source/RobotImporter/URDF/PrefabMakerUtils.h | 6 +++--- .../Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp | 9 +++++---- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp index 4ee0b8fc1..4497d9427 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp @@ -90,7 +90,9 @@ namespace ROS2::PrefabMakerUtils } } - SetEntityParent(entityId, parentEntityId); + // Default the entity world transform to be the same as the parent entity world transform + // Calling SetEntityParent would have the transform be at world origin + SetEntityParentRelative(entityId, parentEntityId); return entityId; } @@ -105,13 +107,13 @@ namespace ROS2::PrefabMakerUtils if (auto* transformComponent = entity->FindComponent(); transformComponent) { - if (!useLocalTransform) + if (useLocalTransform) { - transformComponent->SetParent(parentEntityId); + transformComponent->SetParentRelative(parentEntityId); } else { - transformComponent->SetParentRelative(parentEntityId); + transformComponent->SetParent(parentEntityId); } } } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h index 3a1bea87c..763d557ca 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h @@ -41,14 +41,14 @@ namespace ROS2::PrefabMakerUtils //! @return a result which is either a created prefab entity or an error. AzToolsFramework::Prefab::PrefabEntityResult CreateEntity(AZ::EntityId parentEntityId, const AZStd::string& name); - //! Set the parent entity for an entity. The entity getting parent is expected to be inactive. + //! Set the parent entity for an entity. The entity being attached to the parent entity is expected to be inactive. //! NOTE: This uses the world transform of the entity when updating the transform - //! The entity itself will world location will not change + //! The world location of the entity will not change //! @param entityId the id for entity that needs a parent. //! @param parentEntityId the id for the parent entity. void SetEntityParent(AZ::EntityId entityId, AZ::EntityId parentEntityId); - //! Set the parent entity for an entity. The entity getting parent is expected to be inactive. + //! Set the parent entity for an entity. The entity being attached to the parent is expected to be inactive. //! NOTE: This uses the local transform of the entity when updating the transform //! and therefore allows the entity to relocate based on the parent world transform //! @param entityId the id for entity that needs a parent. diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index d875a4dc9..0aba3363f 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -356,7 +356,7 @@ namespace ROS2 // Use the first joint where this link is a child to locate the parent link pointer. const sdf::Joint* joint = jointsWhereLinkIsChild.front(); - std::string parentLinkName = joint->ParentName(); + std::string parentLinkName = joint->ParentName(); AZStd::string parentName(parentLinkName.c_str(), parentLinkName.size()); // Lookup the entity created from the parent link using the JointMapper to locate the parent SDF link. @@ -384,9 +384,10 @@ namespace ROS2 linkPrefabResult.GetValue().ToString().c_str(), parentEntityIter->second.GetValue().ToString().c_str()); AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s setting parent to %s\n", linkName.c_str(), parentName.c_str()); - // As the link transforms are relative, use SetEntityParentRelative to make sure the link are - // translated, scaled and rotated to correct world location - PrefabMakerUtils::SetEntityParentRelative(linkPrefabResult.GetValue(), parentEntityIter->second.GetValue()); + // The joint hierarchy which specifies how a parent and child link hierarchy is represented in an SDF document + // is used to establish the entity parent child hiearachy, but does not modify the world location of the link entities + // therefore SetEntityParent is used to maintain the world transform of the child link + PrefabMakerUtils::SetEntityParent(linkPrefabResult.GetValue(), parentEntityIter->second.GetValue()); } // Iterate over all the joints and locate the entity associated with the link