Fixed parenting of SDF model and link entity transforms (#591)

* 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.

* 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 <[email protected]>

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())
         {
@@ -90,12 +90,14 @@ 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;
     }
 
-    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 +107,29 @@ namespace ROS2::PrefabMakerUtils
 
         if (auto* transformComponent = entity->FindComponent<AzToolsFramework::Components::TransformComponent>(); transformComponent)
         {
-            transformComponent->SetParent(parentEntityId);
+            if (useLocalTransform)
+            {
+                transformComponent->SetParentRelative(parentEntityId);
+            }
+            else
+            {
+                transformComponent->SetParent(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);

Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h

@@ -41,11 +41,20 @@ 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 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 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.
+    //! @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".

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
                     {
@@ -354,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.
@@ -382,6 +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());
+            // 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());
         }
 

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

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<AZStd::reference_wrapper<const sdf::Model>>;

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);