Updated the Revolute joint maker code to not set a joint limit when t…

…he bounds are (-∞, ∞). Updated the FilePath code to also mark files with the .world extension as SDF files. Signed-off-by: lumberyard-employee-dm <[email protected]>
o3de · Aug 21, 2023 · 68f5bdc · 68f5bdc
1 parent 7a8414d
commit 68f5bdc
Show file tree

Hide file tree

Showing 7 changed files with 20 additions and 14 deletions.
diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp
@@ -46,15 +46,15 @@ namespace ROS2
             const auto type = supportedArticulationType->second;
             articulationLinkConfiguration.m_articulationJointType = type;
             const AZ::Vector3 o3deJointDir{ 1.0, 0.0, 0.0 };
-            AZ::Vector3 jointCoorAxis = AZ::Vector3::CreateZero();
+            AZ::Vector3 jointCoordinateAxis = AZ::Vector3::CreateZero();
             auto quaternion = AZ::Quaternion::CreateIdentity();
 
             const sdf::JointAxis* jointAxis = joint->Axis();
             if (jointAxis != nullptr)
             {
-                jointCoorAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz());
+                jointCoordinateAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz());
                 quaternion =
-                    jointCoorAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoorAxis);
+                    jointCoordinateAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoordinateAxis);
             }
 
             const AZ::Vector3 rotation = quaternion.GetEulerDegrees();

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp
@@ -31,13 +31,13 @@ namespace ROS2
         // converting the unit quaternion to Euler vector.
         const AZ::Vector3 o3deJointDir{ 1.0, 0.0, 0.0 };
         const sdf::JointAxis* jointAxis = joint->Axis();
-        AZ::Vector3 jointCoorAxis = AZ::Vector3::CreateZero();
+        AZ::Vector3 jointCoordinateAxis = AZ::Vector3::CreateZero();
         auto quaternion = AZ::Quaternion::CreateIdentity();
         if (jointAxis != nullptr)
         {
-            jointCoorAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz());
+            jointCoordinateAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz());
             quaternion =
-                jointCoorAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoorAxis);
+                jointCoordinateAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoordinateAxis);
         }
 
         AZ_Printf(
@@ -103,6 +103,8 @@ namespace ROS2
 
                 if (jointAxis != nullptr)
                 {
+                    const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
+                        || jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity()
                     using LimitType = decltype(jointAxis->Upper());
                     const double limitUpper = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
                         ? AZ::RadToDeg(jointAxis->Upper())
@@ -119,12 +121,14 @@ namespace ROS2
                         jointAxis->Lower());
                     PhysX::EditorJointRequestBus::Event(
                         AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()),
-                        [&rotation, &limitLower, &limitUpper](PhysX::EditorJointRequests* editorJointRequest)
+                        [&rotation, enableJointLimits, &limitLower, &limitUpper](PhysX::EditorJointRequests* editorJointRequest)
                         {
                             editorJointRequest->SetVector3Value(PhysX::JointsComponentModeCommon::ParameterNames::Rotation, rotation);
                             editorJointRequest->SetLinearValuePair(
                                 PhysX::JointsComponentModeCommon::ParameterNames::TwistLimits,
                                 PhysX::AngleLimitsFloatPair(limitUpper, limitLower));
+                            editorJointRequest->SetBoolValue(
+                                PhysX::JointsComponentModeCommon::ParameterNames::EnableLimits, enableJointLimits);
                         });
                 }
                 followColliderEntity->Deactivate();

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp
@@ -51,7 +51,7 @@ namespace ROS2
         {
             AZ_Assert(GetFirstModel(), "SDF Model is nullptr");
 
-            AZStd::unordered_map<AZStd::string, const sdf::Material*> materialMap;
+            VisualsMaker::MaterialMap materialMap;
             auto GetVisualsFromModel = [&materialMap](const sdf::Model& model)
             {
                 for (uint64_t linkIndex{}; linkIndex < model.LinkCount(); ++linkIndex)

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h
@@ -33,7 +33,7 @@ namespace ROS2
 
         //! Parse string with URDF data and generate model.
         //! @param xmlString a string that contains URDF data (XML format).
-        //! @param parserConfig structure that contains configuration options for the SDFormater parser
+        //! @param parserConfig structure that contains configuration options for the SDFormatter parser
         //!        The relevant ParserConfig functions for URDF importing are
         //!        URDFPreserveFixedJoint() function to prevent merging of robot links bound by fixed joint
         //!        AddURIPath() function to provide a mapping of package:// and model:// references to the local filesystem

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp
@@ -27,7 +27,7 @@ namespace ROS2
 {
     VisualsMaker::VisualsMaker() = default;
     VisualsMaker::VisualsMaker(
-        AZStd::unordered_map<AZStd::string, const sdf::Material*> materials, const AZStd::shared_ptr<Utils::UrdfAssetMap>& urdfAssetsMapping)
+        MaterialNameMap materials, const AZStd::shared_ptr<Utils::UrdfAssetMap>& urdfAssetsMapping)
         : m_materials(AZStd::move(materials))
         , m_urdfAssetsMapping(urdfAssetsMapping)
     {

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h
@@ -21,9 +21,11 @@ namespace ROS2
     class VisualsMaker
     {
     public:
+        using MaterialNameMap = AZStd::unordered_map<AZStd::string, const sdf::Material*>;
+
         VisualsMaker();
         VisualsMaker(
-            AZStd::unordered_map<AZStd::string, const sdf::Material*> materials,
+            MaterialNameMap materials,
             const AZStd::shared_ptr<Utils::UrdfAssetMap>& urdfAssetsMapping);
 
         //! Add zero, one or many visual elements to a given entity (depending on link content).
@@ -38,7 +40,7 @@ namespace ROS2
         void AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const;
         void AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId) const;
 
-        AZStd::unordered_map<AZStd::string, const sdf::Material*> m_materials;
+        MaterialNameMap m_materials;
         AZStd::shared_ptr<Utils::UrdfAssetMap> m_urdfAssetsMapping;
     };
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/FilePath.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/FilePath.cpp
@@ -18,7 +18,7 @@ namespace ROS2
         {
             if (!filename.HasExtension())
             {
-                return "";
+                return AZStd::string{};
             }
             AZStd::string extension{ filename.Extension().Native() };
             AZStd::to_upper(extension.begin(), extension.end());
@@ -37,7 +37,7 @@ namespace ROS2
 
         bool IsFileSDF(const AZ::IO::Path& filename)
         {
-            return GetCapitalizedExtension(filename) == ".SDF";
+            return GetCapitalizedExtension(filename) == ".SDF" || GetCapitalizedExtension(filename) == ".WORLD";
         }
     } // namespace Utils
 } // namespace ROS2