Add skid steering and adjust vehicle dynamics. (#86)

* Skid steering control component.
Adjust Vehicle Dynamic code to use PhysX joint requests in ROS2 Gem

Changed existing vehicle dynamic code in ROS2 Gem to use JointRequests
from PhysX Gem.

Signed-off-by: Michał Pełka <[email protected]>

* adjusted to review

Signed-off-by: Michał Pełka <[email protected]>

* adjusted to review

Signed-off-by: Michał Pełka <[email protected]>

* Adjusted to reviews.

Signed-off-by: Michał Pełka <[email protected]>

Signed-off-by: Michał Pełka <[email protected]>

Gems/ROS2/Assets/Materials/wheel_material.physxmaterial

@@ -1,17 +1,17 @@
 <ObjectStream version="3">
-    <Class name="PhysX::EditorMaterialAsset" version="2" type="{BC7B88B9-EE31-4FBF-A01E-2A93624C49D3}">
-        <Class name="AssetData" field="BaseClass1" version="1" type="{AF3F7D32-1536-422A-89F3-A11E1F5B5A9C}"/>
-        <Class name="PhysX::MaterialConfiguration" field="MaterialConfiguration" version="1" type="{66213D20-9862-465D-AF4F-2D94317161F6}">
-            <Class name="float" field="DynamicFriction" value="1.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
-            <Class name="float" field="StaticFriction" value="1.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
-            <Class name="float" field="Restitution" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
-            <Class name="unsigned char" field="FrictionCombine" value="2" type="{72B9409A-7D1A-4831-9CFE-FCB3FADD3426}"/>
-            <Class name="unsigned char" field="RestitutionCombine" value="1" type="{72B9409A-7D1A-4831-9CFE-FCB3FADD3426}"/>
-            <Class name="float" field="Density" value="1200.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
-            <Class name="Color" field="DebugColor" value="0.1088426 0.1088426 0.1088426 1.0000000" type="{7894072A-9050-4F0F-901B-34B1A0D29417}"/>
-        </Class>
-        <Class name="PhysicsLegacy::MaterialId" field="LegacyPhysicsMaterialId" version="1" type="{744CCE6C-9F69-4E2F-B950-DAB8514F870B}">
-            <Class name="AZ::Uuid" field="MaterialId" value="{8C7A6011-61C2-46B7-9BF4-8D4DD2A624F1}" type="{E152C105-A133-4D03-BBF8-3D4B2FBA3E2A}"/>
-        </Class>
-    </Class>
+	<Class name="PhysX::EditorMaterialAsset" version="2" type="{BC7B88B9-EE31-4FBF-A01E-2A93624C49D3}">
+		<Class name="AssetData" field="BaseClass1" version="1" type="{AF3F7D32-1536-422A-89F3-A11E1F5B5A9C}"/>
+		<Class name="PhysX::MaterialConfiguration" field="MaterialConfiguration" version="1" type="{66213D20-9862-465D-AF4F-2D94317161F6}">
+			<Class name="float" field="DynamicFriction" value="0.7000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+			<Class name="float" field="StaticFriction" value="0.7000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+			<Class name="float" field="Restitution" value="0.1000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+			<Class name="unsigned char" field="FrictionCombine" value="1" type="{72B9409A-7D1A-4831-9CFE-FCB3FADD3426}"/>
+			<Class name="unsigned char" field="RestitutionCombine" value="1" type="{72B9409A-7D1A-4831-9CFE-FCB3FADD3426}"/>
+			<Class name="float" field="Density" value="120.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+			<Class name="Color" field="DebugColor" value="0.1088426 0.1088426 0.1088426 1.0000000" type="{7894072A-9050-4F0F-901B-34B1A0D29417}"/>
+		</Class>
+		<Class name="PhysicsLegacy::MaterialId" field="LegacyPhysicsMaterialId" version="1" type="{744CCE6C-9F69-4E2F-B950-DAB8514F870B}">
+			<Class name="AZ::Uuid" field="MaterialId" value="{8C7A6011-61C2-46B7-9BF4-8D4DD2A624F1}" type="{E152C105-A133-4D03-BBF8-3D4B2FBA3E2A}"/>
+		</Class>
+	</Class>
 </ObjectStream>

Gems/ROS2/Code/CMakeLists.txt

@@ -32,6 +32,7 @@ ly_add_target(
             Include
         PRIVATE
             Source
+            Source/VehicleDynamics
     BUILD_DEPENDENCIES
         PUBLIC
             AZ::AzCore
@@ -40,6 +41,7 @@ ly_add_target(
             Gem::Atom_Feature_Common.Static
             Gem::Atom_Component_DebugCamera.Static
             Gem::StartingPointInput
+            Gem::PhysX.Static
 )
 
 target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs urdfdom tf2_ros ackermann_msgs gazebo_msgs control_toolbox)

Gems/ROS2/Code/Include/ROS2/VehicleDynamics/VehicleInputControlBus.h

@@ -25,14 +25,26 @@ namespace ROS2::VehicleDynamics
         virtual ~VehicleInputControlRequests() = default;
 
         //! Set target for the vehicle linear speed. It should be realized over time according to drive model.
-        //! @param speedMps is a linear speed in meters per second with the plus sign in the forward direction.
-        virtual void SetTargetLinearSpeed(float speedMps) = 0;
+        //! @param speedMps is a linear speed vectors, unit is meters per second.
+        virtual void SetTargetLinearSpeedV3(const AZ::Vector3& speedMps) = 0;
+
+        //! Set target for the vehicle linear speed. It should be realized over time according to drive model.
+        //! @param speedMpsX is a linear speed in meters per second with the plus sign in the forward direction.
+        virtual void SetTargetLinearSpeed(float speedMpsX) = 0;
 
         //! Steer in a direction given in relative coordinate system (current direction is 0).
         //! @param steering is angle in radians, positive to the right and negative to the left.
         //! @note The actual angle applied is subject to limits and implementation (eg smoothing).
         virtual void SetTargetSteering(float steering) = 0;
 
+        //! Set target for the angular speed. It should be realized over time according to drive model.
+        //! @param rate is an angular speed vector, unit is radians per second.
+        virtual void SetTargetAngularSpeedV3(const AZ::Vector3& rate) = 0;
+
+        //! Set target for the angular speed. It should be realized over time according to drive model.
+        //! @param rateZ is an angular speed in radians per second in up direction .
+        virtual void SetTargetAngularSpeed(float rateZ) = 0;
+
         //! Accelerate without target speed, relative to the limits.
         //! @param accelerationFraction is relative to limits of possible acceleration.
         //! 1 - accelerate as much as possible, -1 - brake as much as possible.
@@ -44,8 +56,12 @@ namespace ROS2::VehicleDynamics
         virtual void SetTargetSteeringFraction(float steeringFraction) = 0;
 
         //! Speed input version which is relative to limits.
-        //! @param speedFraction is -1 to 1, which applies as a fraction of vehicle model speed limits.
-        virtual void SetTargetLinearSpeedFraction(float speedFraction) = 0;
+        //! @param speedFractionX is -1 to 1, which applies as a fraction of vehicle model speed limits.
+        virtual void SetTargetLinearSpeedFraction(float speedFractionX) = 0;
+
+        //! Set target for the angular speed. It should be realized over time according to drive model.
+        //! @param rateFractionZ is an angular speed in radians per second in up direction, fraction of maximum speed.
+        virtual void SetTargetAngularSpeedFraction(float rateFractionZ) = 0;
 
         //! Disables (or enables) the vehicle dynamics
         //! @param disable if set true no torque will be applied

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -19,11 +19,15 @@
 #include <ROS2/Manipulator/MotorizedJointComponent.h>
 #include <RobotControl/Controllers/AckermannController/AckermannControlComponent.h>
 #include <RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h>
+#include <RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h>
 #include <RobotControl/ROS2RobotControlComponent.h>
 #include <RobotImporter/ROS2RobotImporterSystemComponent.h>
 #include <Spawner/ROS2SpawnPointComponent.h>
 #include <Spawner/ROS2SpawnerComponent.h>
+#include <VehicleDynamics/ModelComponents/AckermannModelComponent.h>
+#include <VehicleDynamics/ModelComponents/SkidSteeringModelComponent.h>
 #include <VehicleDynamics/VehicleModelComponent.h>
+
 #include <VehicleDynamics/WheelControllerComponent.h>
 
 namespace ROS2
@@ -54,11 +58,13 @@ namespace ROS2
                   ROS2CameraSensorComponent::CreateDescriptor(),
                   AckermannControlComponent::CreateDescriptor(),
                   RigidBodyTwistControlComponent::CreateDescriptor(),
+                  SkidSteeringControlComponent::CreateDescriptor(),
                   ROS2CameraSensorComponent::CreateDescriptor(),
                   ROS2SpawnerComponent::CreateDescriptor(),
                   ROS2SpawnPointComponent::CreateDescriptor(),
-                  VehicleDynamics::VehicleModelComponent::CreateDescriptor(),
+                  VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(),
                   VehicleDynamics::WheelControllerComponent::CreateDescriptor(),
+                  VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(),
                   MotorizedJointComponent::CreateDescriptor() });
         }
 

Gems/ROS2/Code/Source/ROS2SystemComponent.cpp

@@ -9,6 +9,7 @@
 
 #include <ROS2/Communication/QoS.h>
 #include <ROS2/Communication/TopicConfiguration.h>
+#include <VehicleDynamics/VehicleModelComponent.h>
 
 #include <Atom/RPI.Public/Pass/PassSystemInterface.h>
 
@@ -25,6 +26,7 @@ namespace ROS2
         // Reflect structs not strictly owned by any single component
         QoS::Reflect(context);
         TopicConfiguration::Reflect(context);
+        VehicleDynamics::VehicleModelComponent::Reflect(context);
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serialize->Class<ROS2SystemComponent, AZ::Component>()->Version(0);

Gems/ROS2/Code/Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.cpp

@@ -41,7 +41,7 @@ namespace ROS2
     void AckermannControlComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
         required.push_back(AZ_CRC_CE("ROS2RobotControl"));
-        required.push_back(AZ_CRC_CE("VehicleModelService"));
+        required.push_back(AZ_CRC_CE("AckermannModelService"));
     }
 
     void AckermannControlComponent::AckermannReceived(const AckermannCommandStruct& acs)

Gems/ROS2/Code/Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.cpp

@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "SkidSteeringControlComponent.h"
+#include <AzCore/Component/TransformBus.h>
+#include <AzCore/Math/MathUtils.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
+#include <ROS2/VehicleDynamics/VehicleInputControlBus.h>
+#include <VehicleDynamics/WheelControllerComponent.h>
+
+namespace ROS2
+{
+    void SkidSteeringControlComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<SkidSteeringControlComponent, AZ::Component>()->Version(1);
+            if (AZ::EditContext* editorContext = serialize->GetEditContext())
+            {
+                editorContext->Class<SkidSteeringControlComponent>("Skid steering Twist Control", "")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2");
+            }
+        }
+    }
+
+    void SkidSteeringControlComponent::Activate()
+    {
+        TwistNotificationBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void SkidSteeringControlComponent::Deactivate()
+    {
+        TwistNotificationBus::Handler::BusDisconnect();
+    }
+
+    void SkidSteeringControlComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2RobotControl"));
+        required.push_back(AZ_CRC_CE("SkidSteeringModelService"));
+    }
+
+    void SkidSteeringControlComponent::TwistReceived(const AZ::Vector3& linear, const AZ::Vector3& angular)
+    {
+        // Notify input system for vehicle dynamics. Only speed and steering is currently supported.
+        VehicleDynamics::VehicleInputControlRequestBus::Event(
+            GetEntityId(), &VehicleDynamics::VehicleInputControlRequests::SetTargetLinearSpeedV3, linear);
+        VehicleDynamics::VehicleInputControlRequestBus::Event(
+            GetEntityId(), &VehicleDynamics::VehicleInputControlRequests::SetTargetAngularSpeedV3, angular);
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <ROS2/RobotControl/Twist/TwistBus.h>
+#include <VehicleDynamics/AxleConfiguration.h>
+#include <VehicleDynamics/Utilities.h>
+
+namespace ROS2
+{
+
+    //! Component that contains skid steering model.
+    class SkidSteeringControlComponent
+        : public AZ::Component
+        , private TwistNotificationBus::Handler
+    {
+    public:
+        AZ_COMPONENT(SkidSteeringControlComponent, "{7FEE7851-1284-4AE5-9C2C-763916BFE641}", AZ::Component);
+        SkidSteeringControlComponent() = default;
+
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        // TwistNotificationBus::Handler overrides
+        void TwistReceived(const AZ::Vector3& linear, const AZ::Vector3& angular) override;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp

@@ -45,7 +45,7 @@ namespace ROS2
             //! Clears accumulated log
             void Clear();
 
-            //! Read accumulated log to a string 
+            //! Read accumulated log to a string
             AZStd::string GetLog();
         };
 

Gems/ROS2/Code/Source/VehicleDynamics/DriveModel.cpp

@@ -25,4 +25,23 @@ namespace ROS2::VehicleDynamics
             }
         }
     }
+
+    void DriveModel::SetDisabled(bool isDisabled)
+    {
+        m_disabled = isDisabled;
+    }
+
+    void DriveModel::ApplyInputState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs)
+    {
+        const VehicleInputs filteredInputs = GetVehicleLimitPtr()->LimitState(inputs);
+        ApplyState(filteredInputs, deltaTimeNs);
+    }
+
+    VehicleInputs DriveModel::GetMaximumPossibleInputs() const
+    {
+        auto* vehicleLimitPtr = GetVehicleLimitPtr();
+        AZ_Assert(vehicleLimitPtr, "DriveModel should provide pointer to limits");
+        return vehicleLimitPtr->GetMaximumState();
+    }
+
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/DriveModel.h

@@ -8,8 +8,9 @@
 #pragma once
 
 #include "VehicleConfiguration.h"
-#include "VehicleInputsState.h"
+#include "VehicleInputs.h"
 #include <AzCore/Serialization/SerializeContext.h>
+#include <VehicleDynamics/VehicleModelLimits.h>
 
 namespace ROS2::VehicleDynamics
 {
@@ -25,15 +26,31 @@ namespace ROS2::VehicleDynamics
 
         static void Reflect(AZ::ReflectContext* context);
         virtual ~DriveModel() = default;
-        virtual DriveModel::DriveModelType DriveType() const = 0;
 
         //! Activate the model. Vehicle configuration is to remain the same until another Activate is called.
         //! @param vehicleConfig configuration containing axes and wheels information
         virtual void Activate(const VehicleConfiguration& vehicleConfig) = 0;
 
         //! Applies inputs to the drive. This model will calculate and apply physical forces.
-        //! @param inputs captured state of inputs to use
+        //! @param inputs captured state of inputs to use.
         //! @param deltaTimeNs nanoseconds passed since last call of this function.
-        virtual void ApplyInputState(const VehicleInputsState& inputs, uint64_t deltaTimeNs) = 0;
+        void ApplyInputState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs);
+
+        //! Allows to disable vehicle dynamics.
+        //! @param isDisable true if drive model should be disabled.
+        void SetDisabled(bool isDisable);
+
+        //! Get vehicle maximum limits.
+        VehicleInputs GetMaximumPossibleInputs() const;
+
+    protected:
+        //! Returns pointer to implementation specific Vehicle limits.
+        virtual const VehicleModelLimits* GetVehicleLimitPtr() const = 0;
+
+        //! Apply input to implemented vehicle model.
+        virtual void ApplyState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs) = 0;
+
+        //! True if model is disabled.
+        bool m_disabled{ false };
     };
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.cpp

@@ -12,19 +12,22 @@
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzFramework/Physics/RigidBodyBus.h>
+#include <HingeJointComponent.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
 #include <VehicleDynamics/Utilities.h>
 
 namespace ROS2::VehicleDynamics
 {
     void AckermannDriveModel::Reflect(AZ::ReflectContext* context)
     {
+        AckermannModelLimits::Reflect(context);
         PidConfiguration::Reflect(context);
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serialize->Class<AckermannDriveModel, DriveModel>()
                 ->Version(1)
                 ->Field("SteeringPID", &AckermannDriveModel::m_steeringPid)
-                ->Field("SpeedPID", &AckermannDriveModel::m_speedPid);
+                ->Field("Limits", &AckermannDriveModel::m_limits);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -35,11 +38,7 @@ namespace ROS2::VehicleDynamics
                         &AckermannDriveModel::m_steeringPid,
                         "Steering PID",
                         "Configuration of steering PID controller")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &AckermannDriveModel::m_speedPid,
-                        "Speed PID",
-                        "Configuration of speed PID controller");
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &AckermannDriveModel::m_limits, "Vehicle Limits", "Limits");
             }
         }
     }
@@ -49,11 +48,10 @@ namespace ROS2::VehicleDynamics
         m_driveWheelsData.clear();
         m_steeringData.clear();
         m_vehicleConfiguration = vehicleConfig;
-        m_speedPid.InitializePid();
         m_steeringPid.InitializePid();
     }
 
-    void AckermannDriveModel::ApplyInputState(const VehicleInputsState& inputs, uint64_t deltaTimeNs)
+    void AckermannDriveModel::ApplyState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs)
     {
         if (m_driveWheelsData.empty())
         {
@@ -64,33 +62,30 @@ namespace ROS2::VehicleDynamics
         {
             m_steeringData = VehicleDynamics::Utilities::GetAllSteeringEntitiesData(m_vehicleConfiguration);
         }
-
-        ApplySteering(inputs.m_steering.GetValue(), deltaTimeNs);
-        ApplySpeed(inputs.m_speed.GetValue(), deltaTimeNs);
+        const auto jointPositions = inputs.m_jointRequestedPosition;
+        const float steering = jointPositions.empty() ? 0 : jointPositions.front();
+        ApplySteering(steering, deltaTimeNs);
+        ApplySpeed(inputs.m_speed.GetX(), deltaTimeNs);
     }
 
     void AckermannDriveModel::ApplyWheelSteering(SteeringDynamicsData& wheelData, float steering, double deltaTimeNs)
     {
-        const double deltaTimeSec = double(deltaTimeNs) / 1e9;
-
         const auto& steeringEntity = wheelData.m_steeringEntity;
-        AZ::Vector3 currentSteeringElementRotation;
-        AZ::TransformBus::EventResult(currentSteeringElementRotation, steeringEntity, &AZ::TransformBus::Events::GetLocalRotation);
-        const float currentSteeringAngle = currentSteeringElementRotation.Dot(wheelData.m_turnAxis);
-        const double pidCommand = m_steeringPid.ComputeCommand(steering - currentSteeringAngle, deltaTimeNs);
-        if (AZ::IsClose(pidCommand, 0.0))
-        {
-            return;
-        }
+        const auto& hingeComponent = wheelData.m_hingeJoint;
 
-        const float torque = pidCommand * deltaTimeSec;
-        AZ::Transform steeringElementTransform;
-        AZ::TransformBus::EventResult(steeringElementTransform, steeringEntity, &AZ::TransformBus::Events::GetWorldTM);
-        const auto transformedTorqueVector = steeringElementTransform.TransformVector(wheelData.m_turnAxis * torque);
-        Physics::RigidBodyRequestBus::Event(steeringEntity, &Physics::RigidBodyRequests::ApplyAngularImpulse, transformedTorqueVector);
+        auto id = AZ::EntityComponentIdPair(steeringEntity, hingeComponent);
+
+        PhysX::JointRequestBus::Event(
+            id,
+            [&](PhysX::JointRequests* joint)
+            {
+                double currentSteeringAngle = joint->GetPosition();
+                const double pidCommand = m_steeringPid.ComputeCommand(steering - currentSteeringAngle, deltaTimeNs);
+                joint->SetVelocity(pidCommand);
+            });
     }
 
-    void AckermannDriveModel::ApplySteering(float steering, uint64_t deltaTimeNs)
+    void AckermannDriveModel::ApplySteering(float steering, AZ::u64 deltaTimeNs)
     {
         if (m_disabled)
         {
@@ -113,7 +108,7 @@ namespace ROS2::VehicleDynamics
         ApplyWheelSteering(m_steeringData.back(), outerSteering, deltaTimeNs);
     }
 
-    void AckermannDriveModel::ApplySpeed(float speed, uint64_t deltaTimeNs)
+    void AckermannDriveModel::ApplySpeed(float speed, AZ::u64 deltaTimeNs)
     {
         if (m_disabled)
         {
@@ -125,43 +120,20 @@ namespace ROS2::VehicleDynamics
             return;
         }
 
-        const double deltaTimeSec = double(deltaTimeNs) / 1e9;
         for (const auto& wheelData : m_driveWheelsData)
         {
-            auto wheelEntity = wheelData.m_wheelEntity;
-            AZ::Transform wheelTransform;
-            AZ::TransformBus::EventResult(wheelTransform, wheelEntity, &AZ::TransformBus::Events::GetWorldTM);
-
-            AZ::Transform inverseWheelTransform = wheelTransform.GetInverse();
-            AZ::Vector3 currentAngularVelocity;
-            Physics::RigidBodyRequestBus::EventResult(currentAngularVelocity, wheelEntity, &Physics::RigidBodyRequests::GetAngularVelocity);
-            currentAngularVelocity = inverseWheelTransform.TransformVector(currentAngularVelocity);
-            auto currentAngularSpeedX = currentAngularVelocity.Dot(wheelData.m_driveAxis);
+            const auto wheelEntity = wheelData.m_wheelEntity;
             float wheelRadius = wheelData.m_wheelRadius;
-            if (AZ::IsClose(wheelRadius, 0.0f))
-            {
-                const float defaultFloatRadius = 0.35f;
-                AZ_Warning("ApplySpeed", false, "Wheel radius is zero (or too close to zero), resetting to default %f", defaultFloatRadius);
-                wheelRadius = defaultFloatRadius;
-            }
-
-            auto desiredAngularSpeedX = speed / wheelRadius;
-            double pidCommand = m_speedPid.ComputeCommand(desiredAngularSpeedX - currentAngularSpeedX, deltaTimeNs);
-            if (AZ::IsClose(pidCommand, 0.0))
-            {
-                continue;
-            }
-
-            auto impulse = pidCommand * deltaTimeSec;
-
-            auto transformedTorqueVector = wheelTransform.TransformVector(wheelData.m_driveAxis * impulse);
-            Physics::RigidBodyRequestBus::Event(wheelEntity, &Physics::RigidBodyRequests::ApplyAngularImpulse, transformedTorqueVector);
+            const auto hingeComponent = wheelData.m_hingeJoint;
+            const auto id = AZ::EntityComponentIdPair(wheelEntity, hingeComponent);
+            AZ_Assert(wheelRadius != 0, "wheelRadius must be non-zero");
+            auto desiredAngularSpeedX = (speed / wheelRadius);
+            PhysX::JointRequestBus::Event(id, &PhysX::JointRequests::SetVelocity, desiredAngularSpeedX);
         }
     }
 
-    void AckermannDriveModel::SetDisabled(bool isDisabled)
+    const VehicleModelLimits* AckermannDriveModel::GetVehicleLimitPtr() const
     {
-        m_disabled = isDisabled;
+        return &m_limits;
     }
-
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.h

@@ -10,8 +10,9 @@
 #include <AzCore/Serialization/SerializeContext.h>
 #include <ROS2/VehicleDynamics/DriveModels/PidConfiguration.h>
 #include <VehicleDynamics/DriveModel.h>
+#include <VehicleDynamics/ModelLimits/AckermannModelLimits.h>
 #include <VehicleDynamics/VehicleConfiguration.h>
-#include <VehicleDynamics/VehicleInputsState.h>
+#include <VehicleDynamics/VehicleInputs.h>
 #include <VehicleDynamics/WheelDynamicsData.h>
 
 namespace ROS2::VehicleDynamics
@@ -21,31 +22,27 @@ namespace ROS2::VehicleDynamics
     {
     public:
         AZ_RTTI(AckermannDriveModel, "{104AC31D-E30B-4454-BF42-4FB37B8CFD9B}", DriveModel);
-        DriveModel::DriveModelType DriveType() const override
-        {
-            return DriveModel::DriveModelType::SimplifiedDriveModelType;
-        }
-        //////////////////////////////////////////////////////////////////////////
+
         // DriveModel overrides
         void Activate(const VehicleConfiguration& vehicleConfig) override;
-        void ApplyInputState(const VehicleInputsState& inputs, uint64_t deltaTimeNs) override;
-        //////////////////////////////////////////////////////////////////////////
 
         static void Reflect(AZ::ReflectContext* context);
 
-        void SetDisabled(bool isDisabled);
+    protected:
+        // DriveModel overrides
+        void ApplyState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs) override;
+        const VehicleModelLimits* GetVehicleLimitPtr() const override;
 
     private:
-        void ApplySteering(float steering, uint64_t deltaTimeNs);
-        void ApplySpeed(float speed, uint64_t deltaTimeNs);
+        void ApplySteering(float steering, AZ::u64 deltaTimeNs);
+        void ApplySpeed(float speed, AZ::u64 deltaTimeNs);
         void ApplyWheelSteering(SteeringDynamicsData& wheelData, float steering, double deltaTimeNs);
 
         VehicleConfiguration m_vehicleConfiguration;
         AZStd::vector<WheelDynamicsData> m_driveWheelsData;
         AZStd::vector<SteeringDynamicsData> m_steeringData;
         PidConfiguration m_steeringPid;
-        PidConfiguration m_speedPid;
-        bool m_disabled{ false };
         float m_steeringDeadZone = 0.01;
+        AckermannModelLimits m_limits;
     };
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.cpp

@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "SkidSteeringDriveModel.h"
+#include <AzCore/Component/TransformBus.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <HingeJointComponent.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
+#include <VehicleDynamics/Utilities.h>
+
+namespace ROS2::VehicleDynamics
+{
+    void SkidSteeringDriveModel::Reflect(AZ::ReflectContext* context)
+    {
+        SkidSteeringModelLimits::Reflect(context);
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<SkidSteeringDriveModel, DriveModel>()->Version(1)->Field("Limits", &SkidSteeringDriveModel::m_limits);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<SkidSteeringDriveModel>("Skid Steering Drive Model", "Configuration of a simplified vehicle dynamics drive model")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &SkidSteeringDriveModel::m_limits, "Vehicle Limits", "Limits");
+            }
+        }
+    }
+
+    void SkidSteeringDriveModel::Activate(const VehicleConfiguration& vehicleConfig)
+    {
+        m_config = vehicleConfig;
+        m_wheelsData.clear();
+    }
+
+    void SkidSteeringDriveModel::ApplyState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs)
+    {
+        float angular_speed = inputs.m_angularRates.GetZ();
+        float linear_speed = inputs.m_speed.GetX();
+        // cache PhysX Hinge component IDs
+        if (m_wheelsData.empty())
+        {
+            int driveAxesCount = 0;
+            for (const auto& axle : m_config.m_axles)
+            {
+                if (axle.m_isDrive)
+                {
+                    driveAxesCount++;
+                }
+                for (const auto& wheel : axle.m_axleWheels)
+                {
+                    auto hinge = VehicleDynamics::Utilities::GetWheelPhysxHinge(wheel);
+                    m_wheelsData[wheel] = hinge;
+                }
+                AZ_Warning(
+                    "SkidSteeringDriveModel",
+                    axle.m_axleWheels.size() > 1,
+                    "Axle %s has not enough wheels (%d)",
+                    axle.m_axleTag.c_str(),
+                    axle.m_axleWheels.size());
+            }
+            AZ_Warning("SkidSteeringDriveModel", driveAxesCount != 0, "Skid steering model does not have any drive wheels.");
+        }
+
+        for (size_t axleCount = 0; axleCount < m_config.m_axles.size(); axleCount++)
+        {
+            const auto& axle = m_config.m_axles[axleCount];
+            const auto wheelCount = axle.m_axleWheels.size();
+            if (!axle.m_isDrive || wheelCount < 1)
+            {
+                continue;
+            }
+            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;
+                }
+                float normalizedWheelId = -1.f + 2.f * wheelId / (wheelCount - 1);
+                float wheelBase = normalizedWheelId * m_config.m_wheelbase;
+                AZ_Assert(axle.m_wheelRadius != 0, "axle.m_wheelRadius must be non-zero");
+                float vh = (linear_speed + angular_speed * wheelBase) / axle.m_wheelRadius;
+                PhysX::JointRequestBus::Event(hingePtr->second, &PhysX::JointRequests::SetVelocity, vh);
+            }
+        }
+    }
+
+    const VehicleModelLimits* SkidSteeringDriveModel::GetVehicleLimitPtr() const
+    {
+        return &m_limits;
+    }
+
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/Serialization/SerializeContext.h>
+#include <VehicleDynamics/DriveModel.h>
+
+#include <VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h>
+#include <VehicleDynamics/VehicleConfiguration.h>
+#include <VehicleDynamics/VehicleInputs.h>
+#include <VehicleDynamics/WheelDynamicsData.h>
+
+namespace ROS2::VehicleDynamics
+{
+    //! A simple Ackermann system implementation converting speed and steering inputs into wheel impulse and steering element torque
+    class SkidSteeringDriveModel : public DriveModel
+    {
+    public:
+        AZ_RTTI(SkidSteeringDriveModel, "{04AE1BF2-621A-46C3-B025-E0875856850D}", DriveModel);
+
+        // DriveModel overrides
+        void Activate(const VehicleConfiguration& vehicleConfig) override;
+
+        static void Reflect(AZ::ReflectContext* context);
+
+    protected:
+        // DriveModel overrides
+        void ApplyState(const VehicleInputs& inputs, AZ::u64 deltaTimeNs) override;
+        const VehicleModelLimits* GetVehicleLimitPtr() const override;
+
+    private:
+        SkidSteeringModelLimits m_limits;
+        AZStd::unordered_map<AZ::EntityId, AZ::EntityComponentIdPair> m_wheelsData;
+        VehicleConfiguration m_config;
+    };
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ManualControlEventHandler.h

@@ -54,21 +54,30 @@ namespace ROS2::VehicleDynamics
     class ManualControlEventHandler
     {
     public:
-        void Activate(AZ::EntityId ownerEntity)
+        void Activate(AZ::EntityId ownerEntityId)
         {
             m_eventHandlers.push_back(ManualControlSingleEventHandler(
                 "steering",
-                [ownerEntity](float inputValue)
+                [ownerEntityId](float inputValue)
                 {
-                    VehicleInputControlRequestBus::Event(ownerEntity, &VehicleInputControlRequests::SetTargetSteeringFraction, inputValue);
+                    VehicleInputControlRequestBus::Event(
+                        ownerEntityId, &VehicleInputControlRequests::SetTargetSteeringFraction, inputValue);
                 }));
 
             m_eventHandlers.push_back(ManualControlSingleEventHandler(
                 "accelerate",
-                [ownerEntity](float inputValue)
+                [ownerEntityId](float inputValue)
+                {
+                    VehicleInputControlRequestBus::Event(
+                        ownerEntityId, &VehicleInputControlRequests::SetTargetLinearSpeedFraction, inputValue);
+                }));
+
+            m_eventHandlers.push_back(ManualControlSingleEventHandler(
+                "rotate",
+                [ownerEntityId](float inputValue)
                 {
                     VehicleInputControlRequestBus::Event(
-                        ownerEntity, &VehicleInputControlRequests::SetTargetLinearSpeedFraction, inputValue);
+                        ownerEntityId, &VehicleInputControlRequests::SetTargetAngularSpeedFraction, inputValue);
                 }));
 
             for (auto& handler : m_eventHandlers)

Gems/ROS2/Code/Source/VehicleDynamics/ModelComponents/AckermannModelComponent.cpp

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "AckermannModelComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+
+namespace ROS2::VehicleDynamics
+{
+    void AckermannVehicleModelComponent::Reflect(AZ::ReflectContext* context)
+    {
+        AckermannDriveModel::Reflect(context);
+
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<AckermannVehicleModelComponent, VehicleModelComponent>()->Version(3)->Field(
+                "DriveModel", &AckermannVehicleModelComponent::m_driveModel);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<AckermannVehicleModelComponent>("Ackermann Vehicle Model", "Ackermann vehicle model component")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &AckermannVehicleModelComponent::m_driveModel,
+                        "Drive model",
+                        "Settings of the selected drive model");
+            }
+        }
+    }
+
+    void AckermannVehicleModelComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("AckermannModelService"));
+    }
+
+    void AckermannVehicleModelComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("AckermannModelService"));
+    }
+
+    VehicleDynamics::DriveModel* AckermannVehicleModelComponent::GetDriveModel()
+    {
+        return &m_driveModel;
+    };
+
+    void AckermannVehicleModelComponent::Activate()
+    {
+        VehicleModelComponent::Activate();
+        m_driveModel.Activate(m_vehicleConfiguration);
+    }
+
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelComponents/AckermannModelComponent.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <VehicleDynamics/DriveModel.h>
+#include <VehicleDynamics/DriveModels/AckermannDriveModel.h>
+#include <VehicleDynamics/VehicleModelComponent.h>
+
+namespace ROS2::VehicleDynamics
+{
+    class AckermannVehicleModelComponent : public VehicleModelComponent
+    {
+    public:
+        AZ_COMPONENT(AckermannVehicleModelComponent, "{7618dbb9-fad4-44b3-8587-0d4f97336d3c}", VehicleModelComponent);
+        AckermannVehicleModelComponent() = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        // Component overrides
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        void Activate() override;
+
+    private:
+        VehicleDynamics::AckermannDriveModel m_driveModel;
+
+    protected:
+        // VehicleModelComponent overrides
+        VehicleDynamics::DriveModel* GetDriveModel() override;
+    };
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelComponents/SkidSteeringModelComponent.cpp

@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "SkidSteeringModelComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+
+namespace ROS2::VehicleDynamics
+{
+    void SkidSteeringModelComponent::Reflect(AZ::ReflectContext* context)
+    {
+        SkidSteeringDriveModel::Reflect(context);
+
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<SkidSteeringModelComponent, VehicleModelComponent>()->Version(3)->Field(
+                "DriveModel", &SkidSteeringModelComponent::m_driveModel);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<SkidSteeringModelComponent>("Skid Steering Vehicle Model", "Skid steering vehicle model component")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &SkidSteeringModelComponent::m_driveModel,
+                        "Drive model",
+                        "Settings of the selected drive model");
+            }
+        }
+    }
+
+    void SkidSteeringModelComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("SkidSteeringModelService"));
+    }
+
+    void SkidSteeringModelComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("SkidSteeringModelService"));
+    }
+
+    VehicleDynamics::DriveModel* SkidSteeringModelComponent::GetDriveModel()
+    {
+        return &m_driveModel;
+    };
+
+    void SkidSteeringModelComponent::Activate()
+    {
+        VehicleModelComponent::Activate();
+        m_driveModel.Activate(m_vehicleConfiguration);
+    }
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelComponents/SkidSteeringModelComponent.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <VehicleDynamics/DriveModels/SkidSteeringDriveModel.h>
+#include <VehicleDynamics/VehicleModelComponent.h>
+
+namespace ROS2::VehicleDynamics
+{
+    class SkidSteeringModelComponent : public VehicleModelComponent
+    {
+    public:
+        AZ_COMPONENT(SkidSteeringModelComponent, "{57950C15-F7CF-422B-A452-E4487118F53E}", VehicleModelComponent);
+        SkidSteeringModelComponent() = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        // Component overrides
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+
+        // Component overrides
+        void Activate() override;
+
+    private:
+        VehicleDynamics::SkidSteeringDriveModel m_driveModel;
+
+    protected:
+        // VehicleModelComponent overrides
+        VehicleDynamics::DriveModel* GetDriveModel() override;
+    };
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/AckermannModelLimits.cpp

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "AckermannModelLimits.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+
+namespace ROS2::VehicleDynamics
+{
+    void AckermannModelLimits::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<AckermannModelLimits>()
+                ->Version(1)
+                ->Field("SpeedLimit", &AckermannModelLimits::m_speedLimit)
+                ->Field("SteeringLimit", &AckermannModelLimits::m_steeringLimit);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<AckermannModelLimits>("Skid steering Model Limits", "Limitations of speed, steering angles and other values")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &AckermannModelLimits::m_speedLimit, "Speed Limit", "Max linear speed (meters/sec)")
+                    ->Attribute(AZ::Edit::Attributes::Min, 0.0f)
+                    ->Attribute(AZ::Edit::Attributes::Max, 100.0f)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &AckermannModelLimits::m_steeringLimit, "Steering Limit", "Max steering angle (rad)")
+                    ->Attribute(AZ::Edit::Attributes::Min, 0.0f)
+                    ->Attribute(AZ::Edit::Attributes::Max, 1.57f);
+            }
+        }
+    }
+
+    VehicleInputs AckermannModelLimits::LimitState(const VehicleInputs& inputState) const
+    {
+        VehicleInputs ret = inputState;
+        ret.m_angularRates = AZ::Vector3{ 0 };
+        ret.m_speed = AZ::Vector3{ LimitValue(ret.m_speed.GetX(), m_speedLimit), 0.f, 0.f };
+        if (!ret.m_jointRequestedPosition.empty())
+        {
+            ret.m_jointRequestedPosition.front() = LimitValue(ret.m_jointRequestedPosition.front(), m_steeringLimit);
+        }
+        return ret;
+    }
+
+    VehicleInputs AckermannModelLimits::GetMaximumState() const
+    {
+        VehicleInputs ret;
+        ret.m_speed = { m_speedLimit, 0, 0 };
+        ret.m_jointRequestedPosition = { m_steeringLimit };
+        return ret;
+    }
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/AckermannModelLimits.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/RTTI/TypeInfo.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <VehicleDynamics/VehicleModelLimits.h>
+namespace ROS2::VehicleDynamics
+{
+    //! A structure holding limits of vehicle, including speed and steering limits
+    class AckermannModelLimits : public VehicleModelLimits
+    {
+    public:
+        AZ_RTTI(AckermannModelLimits, "{796928D9-436F-472D-B841-E67F28F9CC82}", VehicleModelLimits);
+        AckermannModelLimits() = default;
+        static void Reflect(AZ::ReflectContext* context);
+
+        // VehicleModelLimits overrides
+        VehicleInputs LimitState(const VehicleInputs& inputState) const;
+        VehicleInputs GetMaximumState() const;
+
+    private:
+        float m_speedLimit = 10.0f; //!< [Mps] Applies to absolute value
+        float m_steeringLimit = 0.7f; //!< [rad] Applies to absolute value
+    };
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.cpp

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "SkidSteeringModelLimits.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/Serialization/SerializeContext.h>
+
+namespace ROS2::VehicleDynamics
+{
+    void SkidSteeringModelLimits::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<SkidSteeringModelLimits>()
+                ->Version(1)
+                ->Field("LinearLimit", &SkidSteeringModelLimits::m_linearLimit)
+                ->Field("AngularLimit", &SkidSteeringModelLimits::m_angularLimit);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<SkidSteeringModelLimits>("Skid steering Model Limits", "Limitations of speed, steering angles and other values")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &SkidSteeringModelLimits::m_linearLimit,
+                        "Linear speed Limit",
+                        "Max linear speed (meters/sec)")
+                    ->Attribute(AZ::Edit::Attributes::Min, 0.0f)
+                    ->Attribute(AZ::Edit::Attributes::Max, 100.0f)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &SkidSteeringModelLimits::m_angularLimit,
+                        "Angular speed Limit",
+                        "Max angular speed (rad/s)")
+                    ->Attribute(AZ::Edit::Attributes::Min, 0.0f)
+                    ->Attribute(AZ::Edit::Attributes::Max, 10.0f);
+            }
+        }
+    }
+
+    VehicleInputs SkidSteeringModelLimits::LimitState(const VehicleInputs& inputState) const
+    {
+        VehicleInputs ret = inputState;
+        ret.m_speed = AZ::Vector3{ LimitValue(ret.m_speed.GetX(), m_linearLimit), 0.f, 0.f };
+        ret.m_angularRates = AZ::Vector3{ 0.f, 0.f, LimitValue(ret.m_angularRates.GetZ(), m_angularLimit) };
+        return ret;
+    }
+
+    VehicleInputs SkidSteeringModelLimits::GetMaximumState() const
+    {
+        VehicleInputs ret;
+        ret.m_speed = { m_linearLimit, 0, 0 };
+        ret.m_angularRates = { 0, 0, m_angularLimit };
+        return ret;
+    }
+
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h

@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/RTTI/TypeInfo.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <VehicleDynamics/VehicleModelLimits.h>
+
+namespace ROS2::VehicleDynamics
+{
+    //! A structure holding limits of skid-steering robot.
+    class SkidSteeringModelLimits : public VehicleModelLimits
+    {
+    public:
+        AZ_RTTI(SkidSteeringModelLimits, "{23420EFB-BB62-48C7-AD37-E50580A53C39}", VehicleModelLimits);
+        SkidSteeringModelLimits() = default;
+        static void Reflect(AZ::ReflectContext* context);
+
+        // VehicleModelLimits overrides
+        VehicleInputs LimitState(const VehicleInputs& inputState) const;
+        VehicleInputs GetMaximumState() const;
+
+    private:
+        float m_linearLimit = 2.0f; //!< [m/s] Maximum travel velocity.
+        float m_angularLimit = 3.5f; //!< [Rad/s] Maximum rotation speed.
+    };
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/Utilities.cpp

@@ -11,6 +11,8 @@
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Component/Entity.h>
 #include <AzCore/std/string/string.h>
+#include <HingeJointComponent.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
 
 namespace ROS2::VehicleDynamics::Utilities
 {
@@ -84,11 +86,28 @@ namespace ROS2::VehicleDynamics::Utilities
                     continue;
                 }
 
-                AZ::Vector3 steeringDir = controllerComponent->m_steeringDir;
-                steeringDir.Normalize();
+                const float steeringScale = controllerComponent->m_steeringScale;
+                PhysX::HingeJointComponent* hingeComponent{ nullptr };
+                AZ::Entity* steeringEntityptr{ nullptr };
+                AZ::ComponentApplicationBus::BroadcastResult(
+                    steeringEntityptr, &AZ::ComponentApplicationRequests::FindEntity, steeringEntity);
+                AZ_Assert(steeringEntityptr, "Cannot find a steering entity for %s", steeringEntity.ToString().c_str());
+                hingeComponent = steeringEntityptr->FindComponent<PhysX::HingeJointComponent>();
+
+                if (!hingeComponent)
+                {
+                    AZ_Warning(
+                        "GetAllSteeringEntitiesData",
+                        false,
+                        "Steering entity specified for WheelController in entity %s does not not have HingeJointComponent, ignoring",
+                        wheel.ToString().c_str());
+                    continue;
+                }
+
                 VehicleDynamics::SteeringDynamicsData steeringData;
+                steeringData.m_steeringScale = steeringScale;
                 steeringData.m_steeringEntity = steeringEntity;
-                steeringData.m_turnAxis = steeringDir;
+                steeringData.m_hingeJoint = hingeComponent->GetId();
                 steeringEntitiesAndAxis.push_back(steeringData);
             }
         }
@@ -129,16 +148,47 @@ namespace ROS2::VehicleDynamics::Utilities
                         axle.m_axleTag.c_str());
                     continue;
                 }
-                AZ::Vector3 driveDir = controllerComponent->m_driveDir;
-                driveDir.Normalize();
+
+                PhysX::HingeJointComponent* hingeComponent{ nullptr };
+                hingeComponent = wheelEntity->FindComponent<PhysX::HingeJointComponent>();
+
+                if (!hingeComponent)
+                {
+                    AZ_Warning(
+                        "GetAllDriveWheelsData",
+                        false,
+                        "Wheel entity for axle %s is missing a HingeJointComponent component, ignoring",
+                        axle.m_axleTag.c_str());
+                    continue;
+                }
 
                 VehicleDynamics::WheelDynamicsData wheelData;
                 wheelData.m_wheelEntity = wheel;
-                wheelData.m_driveAxis = driveDir;
+                wheelData.m_hingeJoint = hingeComponent->GetId();
                 wheelData.m_wheelRadius = axle.m_wheelRadius;
                 driveWheelEntities.push_back(wheelData);
             }
         }
         return driveWheelEntities;
     }
+
+    AZ::EntityComponentIdPair GetWheelPhysxHinge(const AZ::EntityId wheelEntityId)
+    {
+        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();
+        }
+        PhysX::HingeJointComponent* hingeComponent{ nullptr };
+        hingeComponent = wheelEntity->FindComponent<PhysX::HingeJointComponent>();
+        if (!hingeComponent)
+        {
+            AZ_Warning("GetWheelDynamicData", false, "Entity %s has no PhysX::HingeJointComponent", wheelEntityId.ToString().c_str());
+            return AZ::EntityComponentIdPair();
+        }
+        return AZ::EntityComponentIdPair(wheelEntityId, hingeComponent->GetId());
+    }
+
 } // namespace ROS2::VehicleDynamics::Utilities

Gems/ROS2/Code/Source/VehicleDynamics/Utilities.h

@@ -51,4 +51,10 @@ namespace ROS2::VehicleDynamics::Utilities
     //! @returns This function will only return data for properly set up wheels and raise warnings if something is not right.
     //! 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);
+
 } // namespace ROS2::VehicleDynamics::Utilities

Gems/ROS2/Code/Source/VehicleDynamics/VehicleInputs.cpp

@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "VehicleInputs.h"
+
+namespace ROS2::VehicleDynamics
+{
+    template<>
+    AZ::Vector3& InputZeroedOnTimeout<AZ::Vector3>::Zero(AZ::Vector3& input)
+    {
+        input = AZ::Vector3::CreateZero();
+        return input;
+    }
+
+    template<>
+    AZStd::vector<float>& InputZeroedOnTimeout<AZStd::vector<float>>::Zero(AZStd::vector<float>& input)
+    {
+        AZStd::fill(input.begin(), input.end(), 0);
+        return input;
+    }
+
+    VehicleInputs VehicleInputDeadline::GetValueCheckingDeadline()
+    {
+        return VehicleInputs{ m_speed.GetValue(), m_angularRates.GetValue(), m_jointRequestedPosition.GetValue() };
+    }
+} // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/VehicleInputsState.h → Gems/ROS2/Code/Source/VehicleDynamics/VehicleInputs.h

@@ -7,13 +7,15 @@
  */
 #pragma once
 
+#include <AzCore/Math/Vector3.h>
 #include <AzCore/Time/ITime.h>
-#include <type_traits>
+#include <AzCore/std/containers/vector.h>
 
 namespace ROS2::VehicleDynamics
 {
     //! Inputs with an expiration date - effectively is zero after a certain time since update
-    template<typename T, typename = AZStd::enable_if_t<AZStd::is_arithmetic_v<T>>>
+
+    template<typename T>
     class InputZeroedOnTimeout
     {
     public:
@@ -28,16 +30,17 @@ namespace ROS2::VehicleDynamics
             m_lastUpdateUs = GetTimeSinceStartupUs();
         }
 
-        T GetValue() const
+        T& GetValue()
         {
             if (auto timeSinceStartUs = GetTimeSinceStartupUs(); timeSinceStartUs - m_lastUpdateUs > m_timeoutUs)
             {
-                return 0;
+                m_input = Zero(m_input);
             }
             return m_input;
         }
 
     private:
+        T& Zero(T& input);
         int64_t GetTimeSinceStartupUs() const
         {
             return static_cast<int64_t>(AZ::Interface<AZ::ITime>::Get()->GetElapsedTimeUs());
@@ -45,13 +48,24 @@ namespace ROS2::VehicleDynamics
 
         int64_t m_timeoutUs;
         int64_t m_lastUpdateUs = 0;
-        T m_input = 0;
+        T m_input{ 0 };
     };
 
     //! Structure defining the most recent vehicle inputs state
-    struct VehicleInputsState
+    struct VehicleInputs
     {
-        InputZeroedOnTimeout<float> m_speed; //!< Speed measured in m/s
-        InputZeroedOnTimeout<float> m_steering; //!< Steering angle in radians. Negative is right, positive is left,
+        AZ::Vector3 m_speed; //!< Linear speed control measured in m/s
+        AZ::Vector3 m_angularRates; //!< Angular speed control of vehicle
+        AZStd::vector<float> m_jointRequestedPosition; //!< Steering angle in radians. Negative is right, positive is left,
     };
+
+    struct VehicleInputDeadline
+    {
+        InputZeroedOnTimeout<AZ::Vector3> m_speed; //!< Linear speed control measured in m/s
+        InputZeroedOnTimeout<AZ::Vector3> m_angularRates; //!< Linear speed control measured in m/s
+        InputZeroedOnTimeout<AZStd::vector<float>>
+            m_jointRequestedPosition; //!< Steering angle in radians. Negative is right, positive is left,
+        VehicleInputs GetValueCheckingDeadline();
+    };
+
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/VehicleModelComponent.cpp

@@ -10,12 +10,13 @@
 #include "DriveModels/AckermannDriveModel.h"
 #include "Utilities.h"
 #include "VehicleConfiguration.h"
+#include "VehicleModelLimits.h"
 #include <AzCore/Debug/Trace.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/smart_ptr/make_shared.h>
 #include <AzFramework/Physics/RigidBodyBus.h>
-
 namespace ROS2::VehicleDynamics
 {
     void VehicleModelComponent::Activate()
@@ -23,7 +24,6 @@ namespace ROS2::VehicleDynamics
         VehicleInputControlRequestBus::Handler::BusConnect(GetEntityId());
         m_manualControlEventHandler.Activate(GetEntityId());
         AZ::TickBus::Handler::BusConnect();
-        m_driveModel.Activate(m_vehicleConfiguration);
     }
 
     void VehicleModelComponent::Deactivate()
@@ -36,61 +36,40 @@ namespace ROS2::VehicleDynamics
     void VehicleModelComponent::Reflect(AZ::ReflectContext* context)
     {
         VehicleConfiguration::Reflect(context);
-        DriveModel::Reflect(context);
-        AckermannDriveModel::Reflect(context);
         VehicleModelLimits::Reflect(context);
+        DriveModel::Reflect(context);
+
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<VehicleModelComponent, AZ::Component>()
-                ->Version(3)
-                ->Field("VehicleConfiguration", &VehicleModelComponent::m_vehicleConfiguration)
-                ->Field("DriveModel", &VehicleModelComponent::m_driveModel)
-                ->Field("VehicleModelLimits", &VehicleModelComponent::m_vehicleLimits);
+            serialize->Class<VehicleModelComponent, AZ::Component>()->Version(4)->Field(
+                "VehicleConfiguration", &VehicleModelComponent::m_vehicleConfiguration);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
                 ec->Class<VehicleModelComponent>("Vehicle Model", "Customizable vehicle model component")
-                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
-                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
-                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
                         &VehicleModelComponent::m_vehicleConfiguration,
                         "Vehicle settings",
-                        "Vehicle settings including axles and common wheel parameters")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &VehicleModelComponent::m_driveModel,
-                        "Drive model",
-                        "Settings of the selected drive model")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &VehicleModelComponent::m_vehicleLimits,
-                        "Vehicle limits",
-                        "Limits for parameters such as speed and steering angle");
+                        "Vehicle settings including axles and common wheel parameters");
             }
         }
     }
 
-    void VehicleModelComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    void VehicleModelComponent::SetTargetLinearSpeed(float speedMpsX)
     {
-        provided.push_back(AZ_CRC_CE("VehicleModelService"));
+        m_inputsState.m_speed.UpdateValue({ speedMpsX, 0, 0 });
     }
 
-    void VehicleModelComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    void VehicleModelComponent::SetTargetLinearSpeedV3(const AZ::Vector3& speedMps)
     {
-        incompatible.push_back(AZ_CRC_CE("VehicleModelService"));
+        m_inputsState.m_speed.UpdateValue(speedMps);
     }
 
-    void VehicleModelComponent::SetTargetLinearSpeed(float speedMps)
+    void VehicleModelComponent::SetTargetLinearSpeedFraction(float speedFractionX)
     {
-        auto limitedSpeed = VehicleModelLimits::LimitValue(speedMps, m_vehicleLimits.m_speedLimit);
-        m_inputsState.m_speed.UpdateValue(limitedSpeed);
-    }
-
-    void VehicleModelComponent::SetTargetLinearSpeedFraction(float speedFraction)
-    {
-        m_inputsState.m_speed.UpdateValue(speedFraction * m_vehicleLimits.m_speedLimit);
+        const auto& maxState = GetDriveModel()->GetMaximumPossibleInputs();
+        m_inputsState.m_speed.UpdateValue(maxState.m_speed * speedFractionX);
     }
 
     void VehicleModelComponent::SetTargetAccelerationFraction([[maybe_unused]] float accelerationFraction)
@@ -100,23 +79,42 @@ namespace ROS2::VehicleDynamics
 
     void VehicleModelComponent::SetDisableVehicleDynamics(bool isDisable)
     {
-        m_driveModel.SetDisabled(isDisable);
+        GetDriveModel()->SetDisabled(isDisable);
     }
 
     void VehicleModelComponent::SetTargetSteering(float steering)
     {
-        auto limitedSteering = VehicleModelLimits::LimitValue(steering, m_vehicleLimits.m_steeringLimit);
-        m_inputsState.m_steering.UpdateValue(limitedSteering);
+        m_inputsState.m_jointRequestedPosition.UpdateValue({ steering });
     }
 
     void VehicleModelComponent::SetTargetSteeringFraction(float steeringFraction)
     {
-        m_inputsState.m_steering.UpdateValue(steeringFraction * m_vehicleLimits.m_steeringLimit);
+        const auto& maxState = GetDriveModel()->GetMaximumPossibleInputs();
+        if (!maxState.m_jointRequestedPosition.empty())
+        {
+            m_inputsState.m_jointRequestedPosition.UpdateValue({ maxState.m_jointRequestedPosition.front() * steeringFraction });
+        }
     }
 
+    void VehicleModelComponent::SetTargetAngularSpeed(float rateZ)
+    {
+        m_inputsState.m_angularRates.UpdateValue({ 0, 0, rateZ });
+    };
+
+    void VehicleModelComponent::SetTargetAngularSpeedV3(const AZ::Vector3& rate)
+    {
+        m_inputsState.m_angularRates.UpdateValue(rate);
+    };
+
+    void VehicleModelComponent::SetTargetAngularSpeedFraction(float rateFractionZ)
+    {
+        const auto& maxState = GetDriveModel()->GetMaximumPossibleInputs();
+        m_inputsState.m_angularRates.UpdateValue(maxState.m_angularRates * rateFractionZ);
+    };
+
     void VehicleModelComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
     {
         const uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
-        m_driveModel.ApplyInputState(m_inputsState, deltaTimeNs);
+        GetDriveModel()->ApplyInputState(m_inputsState.GetValueCheckingDeadline(), deltaTimeNs);
     }
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/VehicleModelComponent.h

@@ -10,12 +10,12 @@
 #include "DriveModels/AckermannDriveModel.h"
 #include "ManualControlEventHandler.h"
 #include "VehicleConfiguration.h"
-#include "VehicleInputsState.h"
-#include "VehicleModelLimits.h"
+#include "VehicleInputs.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
 #include <AzCore/std/smart_ptr/unique_ptr.h>
 #include <ROS2/VehicleDynamics/VehicleInputControlBus.h>
+#include <VehicleDynamics/VehicleModelLimits.h>
 
 namespace ROS2::VehicleDynamics
 {
@@ -26,36 +26,34 @@ namespace ROS2::VehicleDynamics
         , private AZ::TickBus::Handler
     {
     public:
-        AZ_COMPONENT(VehicleModelComponent, "{7093AE7A-9F64-4C77-8189-02C6B7802C1A}", AZ::Component);
+        AZ_RTTI(VehicleModelComponent, "{7093AE7A-9F64-4C77-8189-02C6B7802C1A}", AZ::Component);
         VehicleModelComponent() = default;
 
-        //////////////////////////////////////////////////////////////////////////
         // Component overrides
         void Activate() override;
         void Deactivate() override;
-        //////////////////////////////////////////////////////////////////////////
 
-        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
-        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
         static void Reflect(AZ::ReflectContext* context);
 
     private:
         void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
 
-        //////////////////////////////////////////////////////////////////////////
         // VehicleInputControlRequestBus::Handler overrides
-        void SetTargetLinearSpeed(float speedMps) override;
+        void SetTargetLinearSpeed(float speedMpsX) override;
+        void SetTargetLinearSpeedV3(const AZ::Vector3& speedMps) override;
         void SetTargetSteering(float steering) override;
+        void SetTargetAngularSpeed(float rateZ) override;
+        void SetTargetAngularSpeedV3(const AZ::Vector3& rate) override;
         void SetTargetAccelerationFraction(float accelerationFraction) override;
         void SetTargetSteeringFraction(float steeringFraction) override;
-        void SetTargetLinearSpeedFraction(float speedFraction) override;
+        void SetTargetLinearSpeedFraction(float speedFractionX) override;
+        void SetTargetAngularSpeedFraction(float rateFractionZ) override;
         void SetDisableVehicleDynamics(bool isDisable) override;
-        //////////////////////////////////////////////////////////////////////////
 
+    protected:
         ManualControlEventHandler m_manualControlEventHandler;
-        VehicleConfiguration m_vehicleConfiguration;
-        VehicleInputsState m_inputsState;
-        AckermannDriveModel m_driveModel;
-        VehicleModelLimits m_vehicleLimits;
+        VehicleInputDeadline m_inputsState;
+        VehicleDynamics::VehicleConfiguration m_vehicleConfiguration;
+        virtual DriveModel* GetDriveModel() = 0;
     };
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/VehicleModelLimits.cpp

@@ -17,23 +17,13 @@ namespace ROS2::VehicleDynamics
     {
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<VehicleModelLimits>()
-                ->Version(2)
-                ->Field("SpeedLimit", &VehicleModelLimits::m_speedLimit)
-                ->Field("SteeringLimit", &VehicleModelLimits::m_steeringLimit);
+            serialize->Class<VehicleModelLimits>()->Version(2);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
                 ec->Class<VehicleModelLimits>("Vehicle Model Limits", "Limitations of speed, steering angles and other values")
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
-                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
-                    ->DataElement(AZ::Edit::UIHandlers::Default, &VehicleModelLimits::m_speedLimit, "Speed Limit", "Max linear speed (mps)")
-                    ->Attribute(AZ::Edit::Attributes::Min, 0.0f)
-                    ->Attribute(AZ::Edit::Attributes::Max, 100.0f)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default, &VehicleModelLimits::m_steeringLimit, "Steering Limit", "Max steering angle (rad)")
-                    ->Attribute(AZ::Edit::Attributes::Min, 0.0f)
-                    ->Attribute(AZ::Edit::Attributes::Max, 1.57f);
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2");
             }
         }
     }

Gems/ROS2/Code/Source/VehicleDynamics/VehicleModelLimits.h

@@ -7,26 +7,35 @@
  */
 #pragma once
 
+#include "VehicleInputs.h"
 #include <AzCore/RTTI/TypeInfo.h>
 #include <AzCore/Serialization/SerializeContext.h>
 
 namespace ROS2::VehicleDynamics
 {
     //! A structure holding limits of vehicle, including speed and steering limits
-    struct VehicleModelLimits
+    class VehicleModelLimits
     {
     public:
-        AZ_TYPE_INFO(VehicleModelLimits, "{76DA392D-64BB-45A8-BC90-84AAE7901991}");
+        AZ_RTTI(VehicleModelLimits, "{76DA392D-64BB-45A8-BC90-84AAE7901991}");
         VehicleModelLimits() = default;
+        virtual ~VehicleModelLimits() = default;
         static void Reflect(AZ::ReflectContext* context);
 
+        //! Limit input state to values that are possible for model.
+        //! @param inputState Input state to filter.
+        //! @returns Filtered, pruned state.
+        virtual VehicleInputs LimitState(const VehicleInputs& inputState) const = 0;
+
+        //! Returns maximal permissible input states.
+        //! @returns VehicleInputsState
+        virtual VehicleInputs GetMaximumState() const = 0;
+
+    protected:
         //! Limit value with a symmetrical range.
         //! @param value Input value.
         //! @param absoluteLimit Limits for value (between -absoluteLimit and absoluteLimit).
         //! @returns A limited value. Always returns either value, -absoluteLimit or absoluteLimit.
         static float LimitValue(float value, float absoluteLimit);
-
-        float m_speedLimit = 10.0f; //!< [Mps] Applies to absolute value
-        float m_steeringLimit = 0.7f; //!< [rad] Applies to absolute value
     };
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/WheelControllerComponent.cpp

@@ -43,10 +43,9 @@ namespace ROS2::VehicleDynamics
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serialize->Class<WheelControllerComponent, AZ::Component>()
-                ->Version(2)
+                ->Version(3)
                 ->Field("SteeringEntity", &WheelControllerComponent::m_steeringEntity)
-                ->Field("DriveDir", &WheelControllerComponent::m_driveDir)
-                ->Field("SteeringDir", &WheelControllerComponent::m_steeringDir);
+                ->Field("SteeringScale", &WheelControllerComponent::m_steeringScale);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -61,13 +60,8 @@ namespace ROS2::VehicleDynamics
                         "Entity which steers the wheel - typically a parent entity")
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
-                        &WheelControllerComponent::m_driveDir,
-                        "Direction of drive axis",
-                        "The direction of torque applied to the wheel entity")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &WheelControllerComponent::m_steeringDir,
-                        "Direction of steering axis",
+                        &WheelControllerComponent::m_steeringScale,
+                        "Scale of steering axis",
                         "The direction of torque applied to the steering entity");
             }
         }

Gems/ROS2/Code/Source/VehicleDynamics/WheelControllerComponent.h

@@ -33,8 +33,7 @@ namespace ROS2::VehicleDynamics
 
         static void Reflect(AZ::ReflectContext* context);
 
-        AZ::EntityId m_steeringEntity; //!< Rigid body to apply torque to.
-        AZ::Vector3 m_driveDir{ 0.0, 0.0, 1.0 }; //!< The direction of torque applied to wheel entity when speed is applied
-        AZ::Vector3 m_steeringDir{ 0.0, 0.0, 1.0 }; //!< The direction of torque applied to steering entity when steering is applied
+        AZ::EntityId m_steeringEntity; //!< Rigid body to apply velocity to.
+        float m_steeringScale{ 1.0f }; //!< The direction of torque applied to steering entity when steering is applied
     };
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/WheelDynamicsData.h

@@ -7,13 +7,16 @@
  */
 #pragma once
 
+#include <AzCore/Component/ComponentBus.h>
+#include <AzCore/Component/EntityId.h>
+
 namespace ROS2::VehicleDynamics
 {
     //! Data structure to pass wheel dynamics data for a single wheel entity.
     struct WheelDynamicsData
     {
         AZ::EntityId m_wheelEntity; //!< An entity which is expected to have a WheelControllerComponent.
-        AZ::Vector3 m_driveAxis{ AZ::Vector3::CreateAxisZ() }; //!< An axis of force application for the wheel to move forward.
+        AZ::ComponentId m_hingeJoint{ AZ::InvalidComponentId }; //!< Steering joint
         float m_wheelRadius{ 0.25f }; //!< Radius of the wheel in meters.
     };
 
@@ -21,8 +24,7 @@ namespace ROS2::VehicleDynamics
     struct SteeringDynamicsData
     {
         AZ::EntityId m_steeringEntity; //!< Steering entity needs to be connected (directly or indirectly) by a Joint with a wheelEntity.
-        AZ::Vector3 m_turnAxis{
-            AZ::Vector3::CreateAxisZ()
-        }; //!< An axis of force application for the steering element to turn the attached wheel sideways.
+        AZ::ComponentId m_hingeJoint{ AZ::InvalidComponentId }; //!< Steering joint
+        float m_steeringScale{ 1.0f }; //!< Scale for direction for the steering element to turn the attached wheel sideways.
     };
 } // namespace ROS2::VehicleDynamics

Gems/ROS2/Code/ros2_files.cmake

@@ -44,6 +44,8 @@ set(FILES
         Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.h
         Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp
         Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h
+        Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.cpp
+        Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h
         Source/RobotControl/ROS2RobotControlComponent.cpp
         Source/RobotControl/ROS2RobotControlComponent.h
         Source/RobotControl/Twist/TwistSubscriptionHandler.cpp
@@ -66,17 +68,28 @@ set(FILES
         Source/VehicleDynamics/DriveModel.h
         Source/VehicleDynamics/DriveModels/AckermannDriveModel.cpp
         Source/VehicleDynamics/DriveModels/AckermannDriveModel.h
+        Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.cpp
+        Source/VehicleDynamics/DriveModels/SkidSteeringDriveModel.h
         Source/VehicleDynamics/DriveModels/PidConfiguration.cpp
         Source/VehicleDynamics/ManualControlEventHandler.h
         Source/VehicleDynamics/Utilities.cpp
         Source/VehicleDynamics/Utilities.h
         Source/VehicleDynamics/VehicleConfiguration.cpp
         Source/VehicleDynamics/VehicleConfiguration.h
-        Source/VehicleDynamics/VehicleInputsState.h
+        Source/VehicleDynamics/VehicleInputs.cpp
+        Source/VehicleDynamics/VehicleInputs.h
         Source/VehicleDynamics/VehicleModelComponent.cpp
         Source/VehicleDynamics/VehicleModelComponent.h
+        Source/VehicleDynamics/ModelComponents/AckermannModelComponent.cpp
+        Source/VehicleDynamics/ModelComponents/AckermannModelComponent.h
+        Source/VehicleDynamics/ModelComponents/SkidSteeringModelComponent.cpp
+        Source/VehicleDynamics/ModelComponents/SkidSteeringModelComponent.h
         Source/VehicleDynamics/VehicleModelLimits.cpp
         Source/VehicleDynamics/VehicleModelLimits.h
+        Source/VehicleDynamics/ModelLimits/AckermannModelLimits.cpp
+        Source/VehicleDynamics/ModelLimits/AckermannModelLimits.h
+        Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.cpp
+        Source/VehicleDynamics/ModelLimits/SkidSteeringModelLimits.h
         Source/VehicleDynamics/WheelControllerComponent.cpp
         Source/VehicleDynamics/WheelControllerComponent.h
         Source/VehicleDynamics/WheelDynamicsData.h