Support for skid-steering and articulations. (o3de#589)

michalpelka · lemonade-dm · web-flow · commit 07221cbd9311 · 2023-11-17T09:03:09.000+01:00
* Fix articulation limits handling in URDFs
* Support vehicle dynamics and articulation

---------

Signed-off-by: Michał Pełka &lt;michal.pelka@robotec.ai&gt;
Co-authored-by: lumberyard-employee-dm &lt;56135373+lumberyard-employee-dm@users.noreply.github.com&gt;
diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp
@@ -62,17 +62,27 @@ namespace ROS2
 
             if (jointAxis)
             {
-                if (type == PhysX::ArticulationJointType::Hinge)
+                using LimitType = decltype(jointAxis->Upper());
+                const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity() ||
+                    jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity();
+                articulationLinkConfiguration.m_isLimited = enableJointLimits;
+                if (enableJointLimits)
                 {
-                    const double limitUpper = AZ::RadToDeg(jointAxis->Upper());
-                    const double limitLower = AZ::RadToDeg(jointAxis->Lower());
-                    articulationLinkConfiguration.m_angularLimitNegative = limitLower;
-                    articulationLinkConfiguration.m_angularLimitPositive = limitUpper;
-                }
-                else if (type == PhysX::ArticulationJointType::Prismatic)
-                {
-                    articulationLinkConfiguration.m_linearLimitLower = jointAxis->Upper();
-                    articulationLinkConfiguration.m_linearLimitUpper = jointAxis->Lower();
+                    if (type == PhysX::ArticulationJointType::Hinge)
+                    {
+                        if (enableJointLimits)
+                        {
+                            const double limitUpper = AZ::RadToDeg(jointAxis->Upper());
+                            const double limitLower = AZ::RadToDeg(jointAxis->Lower());
+                            articulationLinkConfiguration.m_angularLimitNegative = limitLower;
+                            articulationLinkConfiguration.m_angularLimitPositive = limitUpper;
+                        }
+                    }
+                    else if (type == PhysX::ArticulationJointType::Prismatic)
+                    {
+                        articulationLinkConfiguration.m_linearLimitLower = jointAxis->Upper();
+                        articulationLinkConfiguration.m_linearLimitUpper = jointAxis->Lower();
+                    }
                 }
             }
             else
diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.cpp b/Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.cpp
@@ -140,22 +140,10 @@ namespace ROS2::VehicleDynamics
 
         for (const auto& wheelData : m_driveWheelsData)
         {
-            const auto wheelEntity = wheelData.m_wheelEntity;
             float wheelRadius = wheelData.m_wheelRadius;
-            const auto jointComponent = wheelData.m_wheelJoint;
-            const auto id = AZ::EntityComponentIdPair(wheelEntity, jointComponent);
             AZ_Assert(wheelRadius != 0, "wheelRadius must be non-zero");
             auto desiredAngularSpeedX = (m_speedCommand / wheelRadius);
-
-            if (wheelData.m_isArticulation)
-            {
-                PhysX::ArticulationJointRequestBus::Event(
-                    wheelEntity, &PhysX::ArticulationJointRequests::SetDriveTargetVelocity, wheelData.m_axis, desiredAngularSpeedX);
-            }
-            else
-            {
-                PhysX::JointRequestBus::Event(id, &PhysX::JointRequests::SetVelocity, desiredAngularSpeedX);
-            }
+            VehicleDynamics::Utilities::SetWheelRotationSpeed(wheelData, desiredAngularSpeedX);
         }
     }
 
diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.cpp b/Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.cpp
@@ -51,9 +51,8 @@ namespace ROS2::VehicleDynamics
         AZ::ComponentApplicationBus::BroadcastResult(wheelEntityPtr, &AZ::ComponentApplicationRequests::FindEntity, wheelEntityId);
         AZ_Assert(wheelEntityPtr, "The wheelEntity should not be null here");
         auto* wheelControllerComponentPtr = Utils::GetGameOrEditorComponent<WheelControllerComponent>(wheelEntityPtr);
-        auto hingeId = VehicleDynamics::Utilities::GetWheelPhysxHinge(wheelEntityId);
-        AZ::Transform hingeTransform{ AZ::Transform::Identity() };
-        PhysX::JointRequestBus::EventResult(hingeTransform, hingeId, &PhysX::JointRequests::GetTransform);
+        auto wheelData = VehicleDynamics::Utilities::GetWheelData(wheelEntityId, axle.m_wheelRadius);
+        AZ::Transform hingeTransform = Utilities::GetJointTransform(wheelData);
         if (wheelControllerComponentPtr)
         {
             const float normalizedWheelId = -1.f + 2.f * wheelNumber / (wheelCount - 1);
@@ -136,8 +135,8 @@ namespace ROS2::VehicleDynamics
                 }
                 for (const auto& wheel : axle.m_axleWheels)
                 {
-                    auto hinge = VehicleDynamics::Utilities::GetWheelPhysxHinge(wheel);
-                    m_wheelsData[wheel] = hinge;
+                    auto wheelData = VehicleDynamics::Utilities::GetWheelData(wheel, axle.m_wheelRadius);
+                    m_wheelsData[wheel] = wheelData;
                 }
                 AZ_Warning(
                     "SkidSteeringDriveModel",
@@ -158,17 +157,13 @@ namespace ROS2::VehicleDynamics
             }
             for (size_t wheelId = 0; wheelId < wheelCount; wheelId++)
             {
-                const auto& wheel = axle.m_axleWheels[wheelId];
-                auto hingePtr = m_wheelsData.find(wheel);
-                if (hingePtr == m_wheelsData.end())
-                {
-                    continue;
-                }
+                const auto& wheelEntityId = axle.m_axleWheels[wheelId];
                 float normalizedWheelId = -1.f + 2.f * wheelId / (wheelCount - 1);
                 float wheelBase = normalizedWheelId * m_config.m_wheelbase / 2.f;
                 AZ_Assert(axle.m_wheelRadius != 0, "axle.m_wheelRadius must be non-zero");
                 float wheelRate = (m_currentLinearVelocity + m_currentAngularVelocity * wheelBase) / axle.m_wheelRadius;
-                PhysX::JointRequestBus::Event(hingePtr->second, &PhysX::JointRequests::SetVelocity, wheelRate);
+                const auto& wheelData = m_wheelsData[wheelEntityId];
+                VehicleDynamics::Utilities::SetWheelRotationSpeed(wheelData, wheelRate);
             }
         }
     }
diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.h b/Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.h
@@ -52,7 +52,7 @@ namespace ROS2::VehicleDynamics
             int wheelNumber, const AxleConfiguration& axle, const int axisCount) const;
 
         SkidSteeringModelLimits m_limits;
-        AZStd::unordered_map<AZ::EntityId, AZ::EntityComponentIdPair> m_wheelsData;
+        AZStd::unordered_map<AZ::EntityId, VehicleDynamics::WheelDynamicsData> m_wheelsData;
         AZStd::vector<AZStd::tuple<VehicleDynamics::WheelControllerComponent*, AZ::Vector2, AZ::Vector3>> m_wheelColumns;
         VehicleConfiguration m_config;
         float m_currentLinearVelocity = 0.0f;
diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/Utilities.cpp b/Gems/ROS2/Code/Source/VehicleDynamics/Utilities.cpp
@@ -180,73 +180,48 @@ namespace ROS2::VehicleDynamics::Utilities
                     continue;
                 }
 
-                PhysX::HingeJointComponent* hingeComponent{ nullptr };
-                hingeComponent = wheelEntity->FindComponent<PhysX::HingeJointComponent>();
-
-                PhysX::ArticulationLinkComponent* articulation{ nullptr };
-                articulation = wheelEntity->FindComponent<PhysX::ArticulationLinkComponent>();
-
-                if (!hingeComponent && !articulation)
-                {
-                    AZ_Warning(
-                        "GetAllDriveWheelsData",
-                        false,
-                        "Wheel entity for axle %s does not have a HingeJointComponent nor an ArticulationLinkComponent, ignoring",
-                        axle.m_axleTag.c_str());
-                    continue;
-                }
-
-                if (articulation)
-                {
-                    if (articulation->m_config.m_articulationJointType != PhysX::ArticulationJointType::Hinge)
-                    {
-                        AZ_Warning(
-                            "GetAllDriveWheelsData",
-                            false,
-                            "Wheel entity for axle %s has an Articulation Link, but it's not a hinge joint, ignoring",
-                            wheel.ToString().c_str());
-                        continue;
-                    }
-                }
-
-                VehicleDynamics::WheelDynamicsData wheelData;
-                wheelData.m_isArticulation = articulation;
-                wheelData.m_wheelEntity = wheel;
-                if (articulation)
-                {
-                    wheelData.m_wheelJoint = articulation->GetId();
-                    const bool hasFreeAxis = Utils::TryGetFreeArticulationAxis(wheelData.m_wheelEntity, wheelData.m_axis);
-
-                    AZ_Error("VehicleDynamics::Utilities", hasFreeAxis, "Articulation wheel has no free axis somehow");
-                }
-                else
-                {
-                    wheelData.m_wheelJoint = hingeComponent->GetId();
-                }
-                wheelData.m_wheelRadius = axle.m_wheelRadius;
+                VehicleDynamics::WheelDynamicsData wheelData = GetWheelData(wheel, axle.m_wheelRadius);
                 driveWheelEntities.push_back(wheelData);
             }
         }
         return driveWheelEntities;
     }
 
-    AZ::EntityComponentIdPair GetWheelPhysxHinge(const AZ::EntityId wheelEntityId)
+    VehicleDynamics::WheelDynamicsData GetWheelData(const AZ::EntityId wheelEntityId, float wheelRadius)
     {
+        VehicleDynamics::WheelDynamicsData wheelData;
+        wheelData.m_wheelEntity = wheelEntityId;
+        wheelData.m_wheelRadius = wheelRadius;
         AZ::Entity* wheelEntity = nullptr;
         AZ::ComponentApplicationBus::BroadcastResult(wheelEntity, &AZ::ComponentApplicationRequests::FindEntity, wheelEntityId);
         if (!wheelEntity)
         {
             AZ_Warning("GetWheelDynamicData", false, "Entity %s was not found", wheelEntityId.ToString().c_str());
-            return AZ::EntityComponentIdPair();
+            return wheelData;
         }
         PhysX::HingeJointComponent* hingeComponent{ nullptr };
         hingeComponent = wheelEntity->FindComponent<PhysX::HingeJointComponent>();
-        if (!hingeComponent)
+
+        PhysX::ArticulationLinkComponent* articulationComponent{ nullptr };
+        articulationComponent = wheelEntity->FindComponent<PhysX::ArticulationLinkComponent>();
+
+        if (hingeComponent)
+        {
+            wheelData.m_isArticulation = false;
+            wheelData.m_wheelJoint = hingeComponent->GetId();
+            return wheelData;
+        }
+        if (articulationComponent)
         {
-            AZ_Warning("GetWheelDynamicData", false, "Entity %s has no PhysX::HingeJointComponent", wheelEntityId.ToString().c_str());
-            return AZ::EntityComponentIdPair();
+            wheelData.m_isArticulation = true;
+            Utils::TryGetFreeArticulationAxis(wheelEntityId, wheelData.m_axis);
+            wheelData.m_wheelJoint = articulationComponent->GetId();
+            return wheelData;
+
         }
-        return AZ::EntityComponentIdPair(wheelEntityId, hingeComponent->GetId());
+
+        AZ_Warning("GetWheelDynamicData", false, "Entity %s has no PhysX::HingeJointComponent", wheelEntityId.ToString().c_str());
+        return wheelData;
     }
 
     float ComputeRampVelocity(float targetVelocty, float lastVelocity, AZ::u64 deltaTimeNs, float acceleration, float maxVelocity)
@@ -268,4 +243,27 @@ namespace ROS2::VehicleDynamics::Utilities
         }
         return AZStd::clamp(commandVelocity, -maxVelocity, maxVelocity);
     }
+
+    void SetWheelRotationSpeed(const  VehicleDynamics::WheelDynamicsData& data, float wheelRotationSpeed)
+    {
+        if (data.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::Event(
+                data.m_wheelEntity, &PhysX::ArticulationJointRequests::SetDriveTargetVelocity, data.m_axis, wheelRotationSpeed);
+        }
+        else
+        {
+            PhysX::JointRequestBus::Event( AZ::EntityComponentIdPair(data.m_wheelEntity,data.m_wheelJoint), &PhysX::JointRequests::SetVelocity, wheelRotationSpeed);
+        }
+    }
+
+    AZ::Transform GetJointTransform(const VehicleDynamics::WheelDynamicsData& data)
+    {
+        AZ::Transform hingeTransform{ AZ::Transform::Identity() };
+        if (!data.m_isArticulation)
+        {
+            PhysX::JointRequestBus::EventResult(hingeTransform, AZ::EntityComponentIdPair(data.m_wheelEntity,data.m_wheelJoint), &PhysX::JointRequests::GetTransform);
+        }
+        return hingeTransform;
+    }
 } // namespace ROS2::VehicleDynamics::Utilities
diff --git a/Gems/ROS2/Code/Source/VehicleDynamics/Utilities.h b/Gems/ROS2/Code/Source/VehicleDynamics/Utilities.h
@@ -53,10 +53,12 @@ namespace ROS2::VehicleDynamics::Utilities
     //! Wheels need a WheelControllerComponent, and the axle must be a drive axle.
     AZStd::vector<VehicleDynamics::WheelDynamicsData> GetAllDriveWheelsData(const VehicleConfiguration& vehicleConfig);
 
-    //! Get Physx Hinge Joint Component AZ::EntityComponentIdPair from Entity
-    //! @param wheelEntityId id of entity that has Physx Hinge Joint Controller
-    //! @returns EntityComponentIdPair that contains id of Hinge component and @param wheelEntityId
-    AZ::EntityComponentIdPair GetWheelPhysxHinge(const AZ::EntityId wheelEntityId);
+
+    //! Retrieve wheel data for a given wheel entity (components id for joints and free axis)
+    //! @param wheelEntityId Wheel entity to process.
+    //! @param wheelRadius Radius of the wheel in meters.
+    //! @returns struct with wheel data.
+    VehicleDynamics::WheelDynamicsData GetWheelData(const AZ::EntityId wheelEntityId, float wheelRadius);
 
     //! Computes ramped velocity.
     //! @param targetVelocity Last commanded velocity to send to robot (in eg m/s or rad/s)
@@ -67,4 +69,11 @@ namespace ROS2::VehicleDynamics::Utilities
     //! @returns ramped velocity according to time, acceleration and clamped to @param maxVelocity
     float ComputeRampVelocity(float targetVelocity, float lastVelocity, AZ::u64 deltaTimeNs, float acceleration, float maxVelocity);
 
+
+    void SetWheelRotationSpeed(const  VehicleDynamics::WheelDynamicsData& data, float wheelRotationSpeed);
+
+    AZ::Transform GetJointTransform(const VehicleDynamics::WheelDynamicsData& data);
+
+
+
 } // namespace ROS2::VehicleDynamics::Utilities
diff --git a/Gems/ROS2/Registry/articulations.setreg b/Gems/ROS2/Registry/articulations.setreg
@@ -0,0 +1,8 @@
+{
+    "Amazon": {
+        "Physics": {
+            "EnableReducedCoordinateArticulations": true
+            }
+        }
+}
+

Original file line number	Diff line number	Diff line change
`@@ -51,9 +51,8 @@ namespace ROS2::VehicleDynamics`
`51`	`51`	`AZ::ComponentApplicationBus::BroadcastResult(wheelEntityPtr, &AZ::ComponentApplicationRequests::FindEntity, wheelEntityId);`
`52`	`52`	`AZ_Assert(wheelEntityPtr, "The wheelEntity should not be null here");`
`53`	`53`	`auto* wheelControllerComponentPtr = Utils::GetGameOrEditorComponent<WheelControllerComponent>(wheelEntityPtr);`
`54`		`- auto hingeId = VehicleDynamics::Utilities::GetWheelPhysxHinge(wheelEntityId);`
`55`		`- AZ::Transform hingeTransform{ AZ::Transform::Identity() };`
`56`		`- PhysX::JointRequestBus::EventResult(hingeTransform, hingeId, &PhysX::JointRequests::GetTransform);`
	`54`	`+ auto wheelData = VehicleDynamics::Utilities::GetWheelData(wheelEntityId, axle.m_wheelRadius);`
	`55`	`+ AZ::Transform hingeTransform = Utilities::GetJointTransform(wheelData);`
`57`	`56`	`if (wheelControllerComponentPtr)`
`58`	`57`	`{`
`59`	`58`	`const float normalizedWheelId = -1.f + 2.f * wheelNumber / (wheelCount - 1);`
`@@ -136,8 +135,8 @@ namespace ROS2::VehicleDynamics`
`136`	`135`	`}`
`137`	`136`	`for (const auto& wheel : axle.m_axleWheels)`
`138`	`137`	`{`
`139`		`- auto hinge = VehicleDynamics::Utilities::GetWheelPhysxHinge(wheel);`
`140`		`- m_wheelsData[wheel] = hinge;`
	`138`	`+ auto wheelData = VehicleDynamics::Utilities::GetWheelData(wheel, axle.m_wheelRadius);`
	`139`	`+ m_wheelsData[wheel] = wheelData;`
`141`	`140`	`}`
`142`	`141`	`AZ_Warning(`
`143`	`142`	`"SkidSteeringDriveModel",`
`@@ -158,17 +157,13 @@ namespace ROS2::VehicleDynamics`
`158`	`157`	`}`
`159`	`158`	`for (size_t wheelId = 0; wheelId < wheelCount; wheelId++)`
`160`	`159`	`{`
`161`		`- const auto& wheel = axle.m_axleWheels[wheelId];`
`162`		`- auto hingePtr = m_wheelsData.find(wheel);`
`163`		`- if (hingePtr == m_wheelsData.end())`
`164`		`- {`
`165`		`- continue;`
`166`		`- }`
	`160`	`+ const auto& wheelEntityId = axle.m_axleWheels[wheelId];`
`167`	`161`	`float normalizedWheelId = -1.f + 2.f * wheelId / (wheelCount - 1);`
`168`	`162`	`float wheelBase = normalizedWheelId * m_config.m_wheelbase / 2.f;`
`169`	`163`	`AZ_Assert(axle.m_wheelRadius != 0, "axle.m_wheelRadius must be non-zero");`
`170`	`164`	`float wheelRate = (m_currentLinearVelocity + m_currentAngularVelocity * wheelBase) / axle.m_wheelRadius;`
`171`		`- PhysX::JointRequestBus::Event(hingePtr->second, &PhysX::JointRequests::SetVelocity, wheelRate);`
	`165`	`+ const auto& wheelData = m_wheelsData[wheelEntityId];`
	`166`	`+ VehicleDynamics::Utilities::SetWheelRotationSpeed(wheelData, wheelRate);`
`172`	`167`	`}`
`173`	`168`	`}`
`174`	`169`	`}`