RigidBodyTwistControlComponent features (#936)

* Added improvements to rigid body twist controller
* Introduce two mode to RigidBodyTwistControlComponent:

 - Velocity mode: legacy mode where velocity is applied
   using rigid body API
 - Kinematic mode: where velocity is applied as Kinematic
   target
 - Force mode, where velocity is applied as force/torque
   input, computed by PID controllers.

Kinematic controller allows to perform close to ground truth
exercises, but can run havoc in the scene.
Velocity controller causes some issues (applied velocity in all axis),
funny behavior with collisions and doest not work with IMU component.
Finally, the force mode, where no physics tricks are used - it is
the physical way to apply robot locomotion, but requires some tuning.

Signed-off-by: Michał Pełka <[email protected]>
Signed-off-by: Jan Hanca <[email protected]>
Co-authored-by: Jan Hanca <[email protected]>

Gems/ROS2Controllers/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp

@@ -16,11 +16,30 @@
 
 namespace ROS2Controllers
 {
+
+    namespace
+    {
+        AZ::Vector3 ComputeImpulse(const AZ::Vector3& errorVec, AZStd::array<PidConfiguration, 3>& controllers, float fixedDeltaTime)
+        {
+            AZ::Vector3 impulse = AZ::Vector3::CreateZero();
+            for (size_t i = 0; i < 3; ++i)
+            {
+                const auto error = errorVec.GetElement(i);
+                static constexpr float secondsToNanoSeconds = 1'000'000'000.0f;
+                impulse.SetElement(i, controllers[i].ComputeCommand(error, fixedDeltaTime * secondsToNanoSeconds));
+            }
+            return impulse;
+        }
+    } // namespace
+
     void RigidBodyTwistControlComponent::Reflect(AZ::ReflectContext* context)
     {
+        RigidBodyTwistControlComponentConfig::Reflect(context);
+
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<RigidBodyTwistControlComponent, AZ::Component>()->Version(1);
+            serialize->Class<RigidBodyTwistControlComponent, AZ::Component>()->Version(1)->Field(
+                "Config", &RigidBodyTwistControlComponent::m_config);
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
                 ec->Class<RigidBodyTwistControlComponent>("Rigid Body Twist Control", "Simple control through RigidBody")
@@ -28,7 +47,13 @@ namespace ROS2Controllers
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
                     ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
                     ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/RigidBodyTwistControl.svg")
-                    ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg");
+                    ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &RigidBodyTwistControlComponent::m_config,
+                        "Configuration",
+                        "Configuration for the Rigid Body Twist Control Component")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
             }
         }
     }
@@ -37,6 +62,14 @@ namespace ROS2Controllers
     {
         AZ::TickBus::Handler::BusConnect();
         TwistNotificationBus::Handler::BusConnect(GetEntityId());
+        for (auto& controller : m_config.m_linearControllers)
+        {
+            controller.InitializePid();
+        }
+        for (auto& controller : m_config.m_angularControllers)
+        {
+            controller.InitializePid();
+        }
     }
 
     void RigidBodyTwistControlComponent::Deactivate()
@@ -62,7 +95,6 @@ namespace ROS2Controllers
         }
         AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
         AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
-
         AzPhysics::RigidBody* rigidBody = nullptr;
         Physics::RigidBodyRequestBus::EventResult(rigidBody, GetEntityId(), &Physics::RigidBodyRequests::GetRigidBody);
         AZ_Warning("RigidBodyTwistControlComponent", rigidBody, "No rigid body found for entity %s", GetEntity()->GetName().c_str());
@@ -72,23 +104,93 @@ namespace ROS2Controllers
         }
         m_bodyHandle = rigidBody->m_bodyHandle;
         m_sceneFinishSimHandler = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler(
-            [this, sceneInterface]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime)
+            [this]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime)
             {
-                auto* rigidBody = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle);
-                AZ_Assert(sceneInterface, "No body found for previously given handle");
-
-                // Convert local steering to world frame
-                const AZ::Transform robotTransform = rigidBody->GetTransform();
-                const auto linearVelocityGlobal = robotTransform.TransformVector(m_linearVelocityLocal);
-                const auto angularVelocityGlobal = robotTransform.TransformVector(m_angularVelocityLocal);
-                Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, linearVelocityGlobal);
-                Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetAngularVelocity, angularVelocityGlobal);
+                OnSceneSimulationFinish(sceneHandle, fixedDeltaTime);
             },
             aznumeric_cast<int32_t>(AzPhysics::SceneEvents::PhysicsStartFinishSimulationPriority::Components));
         sceneInterface->RegisterSceneSimulationFinishHandler(defaultSceneHandle, m_sceneFinishSimHandler);
         AZ::TickBus::Handler::BusDisconnect();
     }
 
+    void RigidBodyTwistControlComponent::OnSceneSimulationFinish(AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime)
+    {
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface, "No scene interface");
+
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+
+        if (m_bodyHandle == AzPhysics::InvalidSimulatedBodyHandle)
+        {
+            // This component is only interested in the default scene
+            return;
+        }
+        auto* rigidBody = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle);
+        AZ_Assert(rigidBody, "No body found for previously given handle");
+
+        const AZ::Transform robotTransform = rigidBody->GetTransform();
+
+        if (m_config.m_physicalApi == RigidBodyTwistControlComponentConfig::PhysicalApi::Kinematic)
+        {
+            // Convert local steering to world frame
+            const auto linearVelocityGlobal = robotTransform.TransformVector(m_linearVelocityLocal);
+            const auto angularVelocityGlobal = robotTransform.TransformVector(m_angularVelocityLocal);
+
+            // Set the kinematic target for the rigid body
+            // This will move the rigid body to the target position and orientation
+            Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetKinematic, true);
+
+            AZ::Transform kinematicTarget = robotTransform;
+            kinematicTarget.SetTranslation(kinematicTarget.GetTranslation() + linearVelocityGlobal * fixedDeltaTime);
+            kinematicTarget.SetRotation(
+                AZ::Quaternion::CreateFromScaledAxisAngle(angularVelocityGlobal * fixedDeltaTime) * kinematicTarget.GetRotation());
+
+            Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetKinematicTarget, kinematicTarget);
+        }
+        else if (m_config.m_physicalApi == RigidBodyTwistControlComponentConfig::PhysicalApi::Velocity)
+        {
+            // Convert local steering to world frame
+            const auto linearVelocityGlobal = robotTransform.TransformVector(m_linearVelocityLocal);
+            const auto angularVelocityGlobal = robotTransform.TransformVector(m_angularVelocityLocal);
+            Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, linearVelocityGlobal);
+            Physics::RigidBodyRequestBus::Event(GetEntityId(), &Physics::RigidBodyRequests::SetAngularVelocity, angularVelocityGlobal);
+        }
+        else if (m_config.m_physicalApi == RigidBodyTwistControlComponentConfig::PhysicalApi::Force)
+        {
+            const AZ::Transform robotTransformInv = rigidBody->GetTransform().GetInverse();
+            AZ::Vector3 currentLinearVelocityGlob = AZ::Vector3::CreateZero();
+            AZ::Vector3 currentAngularVelocityGlob = AZ::Vector3::CreateZero();
+            Physics::RigidBodyRequestBus::EventResult(
+                currentLinearVelocityGlob, GetEntityId(), &Physics::RigidBodyRequests::GetLinearVelocity);
+            Physics::RigidBodyRequestBus::EventResult(
+                currentAngularVelocityGlob, GetEntityId(), &Physics::RigidBodyRequests::GetAngularVelocity);
+
+            // Convert local steering to world frame
+            const AZ::Vector3 currentLinearVelocityLocal = robotTransformInv.TransformVector(currentLinearVelocityGlob);
+            const AZ::Vector3 angularVelocityLocal = robotTransformInv.TransformVector(currentAngularVelocityGlob);
+
+            const AZ::Vector3 errorLinear = m_linearVelocityLocal - currentLinearVelocityLocal;
+            const AZ::Vector3 errorAngular = m_angularVelocityLocal - angularVelocityLocal;
+
+            // Compute the forces to apply based on the error
+            AZ::Vector3 linearForceLocal = ComputeImpulse(errorLinear, m_config.m_linearControllers, fixedDeltaTime);
+            AZ::Vector3 angularForceLocal = ComputeImpulse(errorAngular, m_config.m_angularControllers, fixedDeltaTime);
+
+            const AZ::Vector3 linearForceGlobal = robotTransform.TransformVector(linearForceLocal);
+            const AZ::Vector3 angularForceGlobal = robotTransform.TransformVector(angularForceLocal);
+
+            Physics::RigidBodyRequestBus::Event(
+                GetEntityId(), &Physics::RigidBodyRequests::ApplyLinearImpulse, linearForceGlobal * fixedDeltaTime);
+            Physics::RigidBodyRequestBus::Event(
+                GetEntityId(), &Physics::RigidBodyRequests::ApplyAngularImpulse, angularForceGlobal * fixedDeltaTime);
+        }
+        else
+        {
+            AZ_Error("RigidBodyTwistControlComponent", false, "Unknown physical API type %d", static_cast<int>(m_config.m_physicalApi));
+        }
+    }
+
     void RigidBodyTwistControlComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
         required.push_back(AZ_CRC_CE("ROS2RobotControl"));

Gems/ROS2Controllers/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h

@@ -7,10 +7,12 @@
  */
 #pragma once
 
+#include "RigidBodyTwistControlComponentConfig.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <ROS2Controllers/RobotControl/Twist/TwistBus.h>
+
 namespace ROS2Controllers
 {
     //! A component with a simple handler for Twist type of control (linear and angular velocities).
@@ -44,9 +46,14 @@ namespace ROS2Controllers
         void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
         //////////////////////////////////////////////////////////////////////////
 
+        void OnSceneSimulationFinish(
+            AzPhysics::SceneHandle sceneHandle,
+            float fixedDeltaTime); //!< Handler for scene simulation finish event
+
         AZ::Vector3 m_linearVelocityLocal{ AZ::Vector3::CreateZero() }; //!< Linear velocity in local frame
         AZ::Vector3 m_angularVelocityLocal{ AZ::Vector3::CreateZero() }; //!< Angular velocity in local frame
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_sceneFinishSimHandler; //!< Handler called after every physics sub-step
         AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; //!< Handle to the body to apply velocities to
+        RigidBodyTwistControlComponentConfig m_config; //!< Configuration for the component
     };
 } // namespace ROS2Controllers

Gems/ROS2Controllers/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponentConfig.cpp

@@ -0,0 +1,88 @@
+/*
+ * 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 "RigidBodyTwistControlComponentConfig.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+namespace ROS2Controllers
+{
+    void RigidBodyTwistControlComponentConfig::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<RigidBodyTwistControlComponentConfig>()
+                ->Version(1)
+                ->Field("PhysicalApi", &RigidBodyTwistControlComponentConfig::m_physicalApi)
+                ->Field("LinerControllers", &RigidBodyTwistControlComponentConfig::m_linearControllers)
+                ->Field("AngularControllers", &RigidBodyTwistControlComponentConfig::m_angularControllers);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<RigidBodyTwistControlComponentConfig>(
+                      "Rigid Body Twist Control Config", "Configuration for Rigid Body Twist Control Component")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::ComboBox,
+                        &RigidBodyTwistControlComponentConfig::m_physicalApi,
+                        "Physical API",
+                        "API to use for applying velocities. Velocity mode directly sets velocities (limited control). Force mode uses PID "
+                        "controllers for better control.")
+                    ->EnumAttribute(PhysicalApi::Kinematic, "Kinematic - Directly sets the kinematic state of the rigid body")
+                    ->EnumAttribute(PhysicalApi::Velocity, "Velocity - Direct velocity control")
+                    ->EnumAttribute(PhysicalApi::Force, "Force - PID controlled force application per axis")
+                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree)
+                    ->UIElement(AZ::Edit::UIHandlers::Label, "", "")
+                    ->Attribute(AZ::Edit::Attributes::NameLabelOverride, "")
+                    ->Attribute(AZ::Edit::Attributes::ValueText, &RigidBodyTwistControlComponentConfig::GetNote)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &RigidBodyTwistControlComponentConfig::m_linearControllers,
+                        "Linear Controllers",
+                        "PID controllers for linear velocities (only used in Force mode)")
+                    ->Attribute(AZ::Edit::Attributes::AutoExpand, false)
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &RigidBodyTwistControlComponentConfig::GetPidControllersVisibility)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &RigidBodyTwistControlComponentConfig::m_angularControllers,
+                        "Angular Controllers",
+                        "PID controllers for angular velocities (only used in Force mode)")
+                    ->Attribute(AZ::Edit::Attributes::AutoExpand, false)
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &RigidBodyTwistControlComponentConfig::GetPidControllersVisibility);
+            }
+        }
+    }
+
+    AZ::u32 RigidBodyTwistControlComponentConfig::GetPidControllersVisibility() const
+    {
+        return m_physicalApi == PhysicalApi::Force ? AZ::Edit::PropertyVisibility::Show : AZ::Edit::PropertyVisibility::Hide;
+    }
+
+    AZStd::string RigidBodyTwistControlComponentConfig::GetNote() const
+    {
+        if (m_physicalApi == PhysicalApi::Force)
+        {
+            return "User defined PID controllers for linear and angular velocities. "
+                   "These controllers are used to apply forces to the rigid body in order to achieve the desired twist control.";
+        }
+        if (m_physicalApi == PhysicalApi::Velocity)
+        {
+            return "Direct velocity control, with collision response. "
+                   "The rigid body will be set to the desired linear and angular velocities."
+                   "This mode limits the usage of odometry and IMU components";
+        }
+        if (m_physicalApi == PhysicalApi::Kinematic)
+        {
+            return "Kinematic mode directly sets the kinematic state of the rigid body. "
+                   "No collision response is applied, and the rigid body will not be affected by physics simulation.";
+        }
+        return "";
+    }
+
+} // namespace ROS2Controllers

Gems/ROS2Controllers/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponentConfig.h

@@ -0,0 +1,47 @@
+/*
+ * 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/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <ROS2Controllers/Controllers/PidConfiguration.h>
+
+namespace ROS2Controllers
+{
+    //! Configuration class for RigidBodyTwistControlComponent
+    class RigidBodyTwistControlComponentConfig
+    {
+    public:
+        enum class PhysicalApi
+        {
+            Velocity,
+            Kinematic,
+            Force
+        };
+
+        AZ_TYPE_INFO(RigidBodyTwistControlComponentConfig, "{8F2E9C4B-1D5A-4B7C-9E8F-3A6B5C7D8E9F}");
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        // Visibility functions for edit context
+        [[nodiscard]] AZ::u32 GetPidControllersVisibility() const;
+        [[nodiscard]] AZStd::string GetNote() const;
+
+        //! Pid controllers for linear velocities - default config for each axis
+        AZStd::array<PidConfiguration, 3> m_linearControllers = { PidConfiguration(500, 50, 0, 0, 0, false, false),
+                                                                  PidConfiguration(500, 50, 0, 0, 0, false, false),
+                                                                  PidConfiguration(0, 0, 0, 0, 0, false, false) };
+
+        //! Pid controllers for angular velocities - default config for each axis
+        AZStd::array<PidConfiguration, 3> m_angularControllers = { PidConfiguration(0, 0, 0, 0, 0, false, false),
+                                                                   PidConfiguration(0, 0, 0, 0, 0, false, false),
+                                                                   PidConfiguration(500, 50, 0, 0, 0, false, false) };
+        PhysicalApi m_physicalApi = PhysicalApi::Velocity; //!< API to use for applying velocities
+    };
+} // namespace ROS2Controllers

Gems/ROS2Controllers/Code/ros2controllers_private_files.cmake

@@ -91,4 +91,6 @@ set(FILES
     Source/VehicleDynamics/WheelControllerComponent.cpp
     Source/VehicleDynamics/WheelControllerComponent.h
     Source/VehicleDynamics/WheelDynamicsData.h
+    Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponentConfig.cpp
+    Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponentConfig.h
 )