Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fixed parenting of SDF model and link entity transforms #591

Merged
merged 2 commits into from Oct 30, 2023
Merged

Fixed parenting of SDF model and link entity transforms #591

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
daa9b72
Fixed parenting of SDF model and link entity transforms
lemonade-dm Oct 25, 2023
65fbddd
Fixed the setup of the parent child entity hierarchy between SDF links
lemonade-dm Oct 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
29 changes: 25 additions & 4 deletions Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp
View file
Open in desktop
Expand Up @@ -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())
{
Expand All @@ -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());
Expand All @@ -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);
Expand Down
11 changes: 10 additions & 1 deletion Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h
View file
Open in desktop
Expand Up @@ -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".
Expand Down
13 changes: 9 additions & 4 deletions Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp
View file
Open in desktop
Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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)
{
Expand All @@ -296,7 +298,7 @@ namespace ROS2
tf.GetRotation().GetY(),
tf.GetRotation().GetZ(),
tf.GetRotation().GetW());
transformInterface->SetWorldTM(tf);
transformInterface->SetLocalTM(tf);
}
else
{
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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());
}

Expand Down
2 changes: 1 addition & 1 deletion Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
View file
Open in desktop
Expand Up @@ -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
Expand Down
10 changes: 5 additions & 5 deletions Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h
View file
Open in desktop
Expand Up @@ -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>>;
Expand Down
6 changes: 3 additions & 3 deletions Gems/ROS2/Code/Tests/UrdfParserTest.cpp
View file
Open in desktop
Expand Up @@ -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);
Expand Down