diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp index 7c0e0774e..4497d9427 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()) { @@ -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(); 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); diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h index 553dffc90..763d557ca 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h +++ b/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". diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index 4f6099b12..0aba3363f 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 { @@ -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()); } 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);