ROS2: Redesign motorized Joint (#253)

* Redesign motorized Joint

Signed-off-by: Aleksander Kamiński <[email protected]>

Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerComponent.h

@@ -0,0 +1,62 @@
+/*
+ * 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 <AzCore/Component/EntityBus.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <ImGuiBus.h>
+#include <ROS2/Manipulation/JointMotorControllerConfiguration.h>
+
+namespace ROS2
+{
+    class JointMotorControllerComponent
+        : public AZ::Component
+        , public AZ::TickBus::Handler
+        , public ImGui::ImGuiUpdateListenerBus::Handler
+        , public AZ::EntityBus::Handler
+    {
+    public:
+        JointMotorControllerComponent() = default;
+        JointMotorControllerComponent(const JointMotorControllerComponent& other) = default;
+        JointMotorControllerComponent(JointMotorControllerComponent&& other) = default;
+        ~JointMotorControllerComponent() = default;
+        AZ_COMPONENT(JointMotorControllerComponent, "{88e725fc-29d8-45b9-b3e8-bd268ad9f413}");
+
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        // ImGui::ImGuiUpdateListenerBus overrides
+        void OnImGuiUpdate() override;
+
+        // EntityBus overrides
+        void OnEntityActivated(const AZ::EntityId& entityId) override;
+
+    protected:
+        AZ::EntityComponentIdPair m_jointComponentIdPair; //!< Joint component managed by the motorized joint.
+        float m_currentPosition{ 0.0f }; //!< Last measured position.
+        float m_currentSpeed{ 0.0f }; //!< Last measured speed.
+
+        JointMotorControllerConfiguration m_jointMotorControllerConfiguration;
+
+    private:
+        virtual float CalculateMotorSpeed([[maybe_unused]] float deltaTime)
+        {
+            return 0.0f;
+        };
+
+        virtual void DisplayControllerParameters(){};
+
+        // AZ::TickBus overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerConfiguration.h

@@ -0,0 +1,27 @@
+/*
+ * 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/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+
+namespace ROS2
+{
+    struct JointMotorControllerConfiguration
+    {
+        AZ_TYPE_INFO(JointMotorControllerConfiguration, "{4358971c-36bd-4e13-8427-74ebba6c6760}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        bool IsDebugModeVisible() const;
+
+        bool m_isDebugController{ false }; //!< Is it a debug controller.
+
+        bool m_debugMode{ false }; //!< Is debug mode activated.
+    };
+
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/ManualMotorControllerComponent.h

@@ -0,0 +1,30 @@
+/*
+ * 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 <ROS2/Manipulation/JointMotorControllerComponent.h>
+
+namespace ROS2
+{
+    class ManualMotorControllerComponent : public JointMotorControllerComponent
+    {
+    public:
+        AZ_COMPONENT(ManualMotorControllerComponent, "{0817634e-4862-4245-a66e-72d1a6939705}", JointMotorControllerComponent);
+
+        ManualMotorControllerComponent();
+        ~ManualMotorControllerComponent() = default;
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        float m_setSpeed{ 0.0f };
+
+        float CalculateMotorSpeed([[maybe_unused]] float deltaTime) override;
+        void DisplayControllerParameters() override;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJointComponent.h

@@ -1,93 +0,0 @@
-/*
- * 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 "MotorizedJointBus.h"
-#include <AzCore/Component/Component.h>
-#include <AzCore/Component/TickBus.h>
-#include <AzCore/Math/Transform.h>
-#include <AzCore/Math/Vector2.h>
-#include <ROS2/Utilities/Controllers/PidConfiguration.h>
-
-namespace ROS2
-{
-    //! A prototype component for simulated joint with a motor.
-    //! It works with either TransformBus or RigidBodyBus.
-    //! TransformBus mode, called `AnimationMode` changes local transform. In this mode, you cannot have a rigid body
-    //! controller enabled. With RigidBodyBus it applies forces and torque according to PID control.
-    //! @note This class is already used through ROS2FrameComponent.
-    class MotorizedJointComponent
-        : public AZ::Component
-        , public AZ::TickBus::Handler
-        , public MotorizedJointRequestBus::Handler
-    {
-    public:
-        AZ_COMPONENT(MotorizedJointComponent, "{AE9207DB-5B7E-4F70-A7DD-C4EAD8DD9403}", AZ::Component);
-
-        MotorizedJointComponent() = default;
-        ~MotorizedJointComponent() = default;
-        //////////////////////////////////////////////////////////////////////////
-        // Component overrides
-        void Activate() override;
-        void Deactivate() override;
-        static void Reflect(AZ::ReflectContext* context);
-        //////////////////////////////////////////////////////////////////////////
-
-        ////////////////////////////////////////////////////////////////////////
-        // MotorizedJointRequestBus::Handler overrides
-        void SetSetpoint(float setpoint) override;
-        float GetSetpoint() override;
-        float GetError() override;
-        float GetCurrentMeasurement() override;
-        ////////////////////////////////////////////////////////////////////////
-
-        //! Get a degree of freedom direction.
-        //! @returns direction of joint movement in global coordinates.
-        AZ::Vector3 GetDir() const
-        {
-            return m_jointDir;
-        };
-
-    private:
-        float ComputeMeasurement(AZ::ScriptTimePoint time);
-        void SetVelocity(float velocity /* m/s */, float deltaTime /* seconds */);
-        void ApplyLinVelAnimation(float velocity /* m/s */, float deltaTime /* seconds */);
-        void ApplyLinVelRigidBodyImpulse(float velocity /* m/s */, float deltaTime /* seconds */);
-        void ApplyLinVelRigidBody(float velocity /* m/s */, float deltaTime /* seconds */);
-        //////////////////////////////////////////////////////////////////////////
-        // AZ::TickBus::Handler overrides
-        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
-        //////////////////////////////////////////////////////////////////////////
-
-        AZ::Vector3 m_jointDir{ 0.f, 0.f, 1.f }; //!< Direction of joint movement in parent frame of reference, used to compute measurement.
-        AZ::Vector3 m_effortAxis{ 0.f, 0.f, 1.f }; //!< Direction of force or torque application in owning entity frame of reference.
-        AZStd::pair<float, float> m_limits{ -0.5f, 0.5f }; //!< limits of joint, the force is applied only when joint is within limits.
-        Controllers::PidConfiguration m_pidPos; //!< PID controller for position.
-
-        bool m_linear{ true }; //!< Linear mode. The force is applied through RigidBodyBus.
-        bool m_animationMode{ true }; //!< Use TransformBus (animation mode, no physics) instead of RigidBodyBus.
-
-        bool m_testSinusoidal{ true }; //!< Enable sinusoidal signal generator to setpoint (for tuning).
-        float m_sinAmplitude{ 0.5 }; //!< Amplitude of test signal generator.
-        float m_sinDC{ 0.25 }; //!< DC of test signal generator.
-        float m_sinFreq{ 0.1 }; //!< Frequency of test signal generator.
-
-        float m_zeroOffset{ 0.f }; //!< offset added to setpoint.
-        float m_setpoint{ 0 }; //!< Desired local position.
-        float m_error{ 0 }; //!< Current error (difference between control value and measurement).
-        float m_currentPosition{ 0 }; //!< Last measured position.
-        float m_currentVelocity{ 0 }; //!< Last measured velocity.
-        double m_lastMeasurementTime{ 0 }; //!< Last measurement time in seconds.
-
-        AZ::EntityId m_debugDrawEntity; //!< Optional Entity that allows to visualize desired setpoint value.
-        AZ::Transform m_debugDrawEntityInitialTransform; //!< Initial transform of m_debugDrawEntity.
-        bool m_debugPrint{ false }; //!< Print debug info to the console.
-
-        AZ::EntityId m_measurementReferenceEntity; //!< Entity used for reference for measurements. Defaults to parent entity.
-    };
-} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJointBus.h → Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerBus.h

@@ -13,15 +13,15 @@
 
 namespace ROS2
 {
-    //! Interface to communicate with motorized joints.
-    //! It allows to apply setpoint and tracking performance of the PID controller.
-    class MotorizedJointRequest : public AZ::EBusTraits
+    //! Interface to communicate with a PID motor controller.
+    //! It allows to apply a setpoint and track performance of the controller.
+    class PidMotorControllerRequests : public AZ::EBusTraits
     {
     public:
         static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
         using BusIdType = AZ::EntityId;
 
-        virtual ~MotorizedJointRequest() = default;
+        virtual ~PidMotorControllerRequests() = default;
 
         //! Set current setpoint value for position controller.
         //! The setpoint is the desired position for a simulated actuator.
@@ -53,5 +53,5 @@ namespace ROS2
         virtual float GetError() = 0;
     };
 
-    using MotorizedJointRequestBus = AZ::EBus<MotorizedJointRequest>;
-} // namespace ROS2
+    using PidMotorControllerRequestBus = AZ::EBus<PidMotorControllerRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerComponent.h

@@ -0,0 +1,49 @@
+/*
+ * 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 <AzFramework/Entity/EntityDebugDisplayBus.h>
+#include <ROS2/Manipulation/JointMotorControllerComponent.h>
+#include <ROS2/Manipulation/PidMotorControllerBus.h>
+#include <ROS2/Utilities/Controllers/PidConfiguration.h>
+
+namespace ROS2
+{
+    class PidMotorControllerComponent
+        : public JointMotorControllerComponent
+        , public PidMotorControllerRequestBus::Handler
+    {
+    public:
+        AZ_COMPONENT(PidMotorControllerComponent, "{ac1d057f-a6ad-4a26-b44f-0ebda2f5f526}", JointMotorControllerComponent);
+
+        PidMotorControllerComponent() = default;
+        ~PidMotorControllerComponent() = default;
+        static void Reflect(AZ::ReflectContext* context);
+
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+
+        // PidMotorControllerBus overrides
+        void SetSetpoint(float setpoint) override;
+        float GetSetpoint() override;
+        float GetCurrentMeasurement() override;
+        float GetError() override;
+
+    private:
+        Controllers::PidConfiguration m_pidPos; //!< PID controller for position.
+        float m_zeroOffset{ 0.0f }; //!< Offset added to setpoint.
+        float m_setPoint{ 0.0f }; //!< Desired local position.
+        float m_error{ 0.0f }; //!< Current error (difference between control value and measurement).
+
+        // JointMotorControllerComponent overrides
+        float CalculateMotorSpeed([[maybe_unused]] float deltaTime) override;
+        void DisplayControllerParameters() override;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointMotorControllerComponent.cpp

@@ -0,0 +1,111 @@
+/*
+ * 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 <AzFramework/Entity/EntityDebugDisplayBus.h>
+#include <HingeJointComponent.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
+#include <PrismaticJointComponent.h>
+#include <ROS2/Manipulation/JointMotorControllerComponent.h>
+#include <imgui/imgui.h>
+
+namespace ROS2
+{
+    void JointMotorControllerComponent::Activate()
+    {
+        AZ::TickBus::Handler::BusConnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
+        AZ::EntityBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void JointMotorControllerComponent::Deactivate()
+    {
+        ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+    }
+
+    void JointMotorControllerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        JointMotorControllerConfiguration::Reflect(context);
+
+        if (auto* serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<JointMotorControllerComponent, AZ::Component>()->Version(2)->Field(
+                "JointMotorControllerConfiguration", &JointMotorControllerComponent::m_jointMotorControllerConfiguration);
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<JointMotorControllerComponent>(
+                      "Joint Motor Controller Component", "Base component for motor controller components.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &JointMotorControllerComponent::m_jointMotorControllerConfiguration,
+                        "Motor Controller Configuration",
+                        "Motor Controller Configuration");
+            }
+        }
+    }
+
+    void JointMotorControllerComponent::OnImGuiUpdate()
+    {
+        if (!m_jointComponentIdPair.GetEntityId().IsValid() ||
+            !(m_jointMotorControllerConfiguration.m_isDebugController || m_jointMotorControllerConfiguration.m_debugMode))
+        {
+            return;
+        }
+
+        AZStd::pair<float, float> limits{ 0.0f, 0.0f };
+        PhysX::JointRequestBus::EventResult(limits, m_jointComponentIdPair, &PhysX::JointRequests::GetLimits);
+        PhysX::JointRequestBus::EventResult(m_currentSpeed, m_jointComponentIdPair, &PhysX::JointRequests::GetVelocity);
+
+        AZStd::string s =
+            AZStd::string::format("Joint Motor Controller %s:%s", GetEntity()->GetName().c_str(), GetEntity()->GetId().ToString().c_str());
+        ImGui::Begin(s.c_str());
+
+        ImGui::PushItemWidth(200.0f);
+        ImGui::SliderFloat("Position", &m_currentPosition, limits.first, limits.second);
+        ImGui::SliderFloat("Speed", &m_currentSpeed, -5.0f, 5.0f);
+        ImGui::PopItemWidth();
+
+        DisplayControllerParameters();
+
+        ImGui::End();
+    }
+
+    void JointMotorControllerComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (!m_jointComponentIdPair.GetEntityId().IsValid())
+        {
+            return;
+        }
+
+        PhysX::JointRequestBus::EventResult(m_currentPosition, m_jointComponentIdPair, &PhysX::JointRequests::GetPosition);
+        float setSpeed = CalculateMotorSpeed(deltaTime);
+        PhysX::JointRequestBus::Event(m_jointComponentIdPair, &PhysX::JointRequests::SetVelocity, setSpeed);
+    }
+
+    void JointMotorControllerComponent::OnEntityActivated(const AZ::EntityId& entityId)
+    {
+        AZ::ComponentId componentId;
+        if (auto* prismaticJointComponent = GetEntity()->FindComponent<PhysX::PrismaticJointComponent>(); prismaticJointComponent)
+        {
+            componentId = prismaticJointComponent->GetId();
+        }
+        else if (auto* hingeJointComponent = GetEntity()->FindComponent<PhysX::HingeJointComponent>(); hingeJointComponent)
+        {
+            componentId = hingeJointComponent->GetId();
+        }
+        else
+        {
+            AZ_Warning(
+                "MotorizedJointComponent",
+                false,
+                "Entity with ID %s either has no PhysX::Joint component or the joint is neither a Prismatic nor a Hinge Joint",
+                GetEntityId().ToString().c_str());
+        }
+
+        m_jointComponentIdPair = { GetEntityId(), componentId };
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointMotorControllerConfiguration.cpp

@@ -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
+ *
+ */
+#include <ROS2/Manipulation/JointMotorControllerConfiguration.h>
+
+namespace ROS2
+{
+    bool JointMotorControllerConfiguration::IsDebugModeVisible() const
+    {
+        return !m_isDebugController;
+    }
+
+    void JointMotorControllerConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<JointMotorControllerConfiguration>()->Version(0)->Field(
+                "DebugMode", &JointMotorControllerConfiguration::m_debugMode);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<JointMotorControllerConfiguration>("Joint Motor Controller Configuration", "Motor Controller Configuration")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &JointMotorControllerConfiguration::m_debugMode, "Debug Mode", "Enable Debug Mode")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &JointMotorControllerConfiguration::IsDebugModeVisible);
+            }
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/ManualMotorControllerComponent.cpp

@@ -0,0 +1,55 @@
+/*
+ * 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 <ROS2/Manipulation/ManualMotorControllerComponent.h>
+#include <imgui/imgui.h>
+
+namespace ROS2
+{
+    ManualMotorControllerComponent::ManualMotorControllerComponent()
+    {
+        m_jointMotorControllerConfiguration.m_isDebugController = true;
+    }
+
+    void ManualMotorControllerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto* serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<ManualMotorControllerComponent, JointMotorControllerComponent>()->Version(2);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<ManualMotorControllerComponent>("Manual Motor Controller", "Debug motor controller used via ImGui.")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"));
+            }
+        }
+    }
+
+    float ManualMotorControllerComponent::CalculateMotorSpeed([[maybe_unused]] float deltaTime)
+    {
+        return m_setSpeed;
+    }
+
+    void ManualMotorControllerComponent::DisplayControllerParameters()
+    {
+        ImGui::PushItemWidth(200.0f);
+        ImGui::SliderFloat(
+            "SetSpeed",
+            &m_setSpeed,
+            -5.0f,
+            5.0f);
+
+        ImGui::PopItemWidth();
+
+        if (ImGui::Button("Zero"))
+        {
+            m_setSpeed = 0.0f;
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/MotorizedJointComponent.cpp

@@ -1,273 +0,0 @@
-/*
- * 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 <AzCore/Component/Entity.h>
-#include <AzCore/Component/TransformBus.h>
-#include <AzCore/Serialization/EditContext.h>
-#include <AzFramework/Physics/Components/SimulatedBodyComponentBus.h>
-#include <AzFramework/Physics/RigidBodyBus.h>
-#include <ROS2/Manipulation/MotorizedJointComponent.h>
-
-namespace ROS2
-{
-    void MotorizedJointComponent::Activate()
-    {
-        AZ::TickBus::Handler::BusConnect();
-        m_pidPos.InitializePid();
-        if (m_debugDrawEntity.IsValid())
-        {
-            AZ::TransformBus::EventResult(m_debugDrawEntityInitialTransform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM);
-        }
-        MotorizedJointRequestBus::Handler::BusConnect(m_entity->GetId());
-    }
-
-    void MotorizedJointComponent::Deactivate()
-    {
-        AZ::TickBus::Handler::BusDisconnect();
-        MotorizedJointRequestBus::Handler::BusDisconnect();
-    }
-
-    void MotorizedJointComponent::Reflect(AZ::ReflectContext* context)
-    {
-        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
-        {
-            serialize->Class<MotorizedJointComponent, AZ::Component>()
-                ->Version(2)
-                ->Field("JointAxis", &MotorizedJointComponent::m_jointDir)
-                ->Field("EffortAxis", &MotorizedJointComponent::m_effortAxis)
-                ->Field("Limit", &MotorizedJointComponent::m_limits)
-                ->Field("Linear", &MotorizedJointComponent::m_linear)
-                ->Field("AnimationMode", &MotorizedJointComponent::m_animationMode)
-                ->Field("ZeroOffset", &MotorizedJointComponent::m_zeroOffset)
-                ->Field("PidPosition", &MotorizedJointComponent::m_pidPos)
-                ->Field("DebugDrawEntity", &MotorizedJointComponent::m_debugDrawEntity)
-                ->Field("TestSinActive", &MotorizedJointComponent::m_testSinusoidal)
-                ->Field("TestSinAmplitude", &MotorizedJointComponent::m_sinAmplitude)
-                ->Field("TestSinFreq", &MotorizedJointComponent::m_sinFreq)
-                ->Field("TestSinDC", &MotorizedJointComponent::m_sinDC)
-                ->Field("DebugPrint", &MotorizedJointComponent::m_debugPrint)
-                ->Field("OverrideParent", &MotorizedJointComponent::m_measurementReferenceEntity);
-
-            if (AZ::EditContext* ec = serialize->GetEditContext())
-            {
-                ec->Class<MotorizedJointComponent>("MotorizedJointComponent", "MotorizedJointComponent")
-                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "MotorizedJointComponent")
-                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
-                    ->Attribute(AZ::Edit::Attributes::Category, "MotorizedJointComponent")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_jointDir,
-                        "Joint Dir.",
-                        "Direction of joint in parent's reference frame.")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_effortAxis,
-                        "Effort Dir.",
-                        "Desired direction of force/torque vector that is applied to rigid body.")
-
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_limits,
-                        "ControllerLimits",
-                        "When measurement is outside the limits, ")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_debugDrawEntity,
-                        "Setpoint",
-                        "Allows to apply debug setpoint visualizer")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_zeroOffset,
-                        "Zero Off.",
-                        "Allows to change offset of zero to set point")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_linear,
-                        "Linear joint",
-                        "Applies linear force instead of torque")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_animationMode,
-                        "Animation mode",
-                        "In animation mode, the transform API is used instead of Rigid Body. "
-                        "If this property is set to true the Rigid Body Component should be disabled.")
-                    ->DataElement(AZ::Edit::UIHandlers::Default, &MotorizedJointComponent::m_pidPos, "PidPosition", "PidPosition")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_testSinusoidal,
-                        "SinusoidalTest",
-                        "Allows to apply sinusoidal test signal")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_sinAmplitude,
-                        "Amplitude",
-                        "Amplitude of sinusoidal test signal.")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_sinFreq,
-                        "Frequency",
-                        "Frequency of sinusoidal test signal.")
-                    ->DataElement(AZ::Edit::UIHandlers::Default, &MotorizedJointComponent::m_sinDC, "DC", "DC of sinusoidal test signal.")
-                    ->DataElement(AZ::Edit::UIHandlers::Default, &MotorizedJointComponent::m_debugPrint, "Debug", "Print debug to console")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &MotorizedJointComponent::m_measurementReferenceEntity,
-                        "Step Parent",
-                        "Allows to override a parent to get correct measurement");
-            }
-        }
-    }
-    void MotorizedJointComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
-    {
-        const float measurement = ComputeMeasurement(time);
-        if (m_testSinusoidal)
-        {
-            m_setpoint = m_sinDC + m_sinAmplitude * AZ::Sin(m_sinFreq * time.GetSeconds());
-        }
-        const float control_position_error = (m_setpoint + m_zeroOffset) - measurement;
-        m_error = control_position_error;
-
-        if (m_debugDrawEntity.IsValid())
-        {
-            if (m_linear)
-            {
-                AZ::Transform transform = AZ::Transform::Identity();
-                transform.SetTranslation(m_jointDir * (m_setpoint + m_zeroOffset));
-                AZ::TransformBus::Event(
-                    m_debugDrawEntity, &AZ::TransformBus::Events::SetLocalTM, transform * m_debugDrawEntityInitialTransform);
-            }
-            else
-            {
-                AZ_Assert(false, "Not implemented");
-            }
-        }
-
-        const uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
-        float speed_control = m_pidPos.ComputeCommand(control_position_error, deltaTimeNs);
-
-        if (measurement <= m_limits.first)
-        {
-            // allow only positive control
-            speed_control = AZStd::max(0.f, speed_control);
-        }
-        else if (measurement >= m_limits.second)
-        {
-            // allow only negative control
-            speed_control = AZStd::min(0.f, speed_control);
-        }
-
-        if (m_debugPrint)
-        {
-            AZ_Printf(
-                "MotorizedJointComponent",
-                " %s | pos: %f | err: %f | cntrl : %f | set : %f |\n",
-                GetEntity()->GetName().c_str(),
-                measurement,
-                control_position_error,
-                speed_control,
-                m_setpoint);
-        }
-        SetVelocity(speed_control, deltaTime);
-    }
-
-    float MotorizedJointComponent::ComputeMeasurement(AZ::ScriptTimePoint time)
-    {
-        AZ::Transform transform;
-        if (!m_measurementReferenceEntity.IsValid())
-        {
-            AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM);
-        }
-        else
-        {
-            AZ::Transform transformStepParent;
-            AZ::TransformBus::EventResult(transformStepParent, m_measurementReferenceEntity, &AZ::TransformBus::Events::GetWorldTM);
-            AZ::Transform transformStepChild;
-            AZ::TransformBus::EventResult(transformStepChild, GetEntityId(), &AZ::TransformBus::Events::GetWorldTM);
-            transform = transformStepParent.GetInverse() * transformStepChild;
-        }
-        if (m_linear)
-        {
-            const float last_position = m_currentPosition;
-            m_currentPosition = transform.GetTranslation().Dot(this->m_jointDir);
-            if (m_lastMeasurementTime > 0)
-            {
-                double delta_time = time.GetSeconds() - m_lastMeasurementTime;
-                m_currentVelocity = (m_currentPosition - last_position) / delta_time;
-            }
-            m_lastMeasurementTime = time.GetSeconds();
-            return m_currentPosition;
-        }
-        AZ_Assert(false, "Measurement computation for rotation is not implemented");
-        return 0;
-    }
-
-    void MotorizedJointComponent::SetVelocity(float velocity, float deltaTime)
-    {
-        if (m_animationMode)
-        {
-            ApplyLinVelAnimation(velocity, deltaTime);
-        }
-        else
-        {
-            deltaTime = AZStd::min(deltaTime, 0.1f); // this affects applied force. Need to prevent value that is too large.
-            if (m_linear)
-            {
-                ApplyLinVelRigidBodyImpulse(velocity, deltaTime);
-            }
-        }
-    }
-
-    void MotorizedJointComponent::ApplyLinVelAnimation(float velocity, float deltaTime)
-    {
-        AZ::Transform transform;
-        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM);
-        transform.SetTranslation(transform.GetTranslation() + velocity * m_jointDir * deltaTime);
-        AZ::TransformBus::Event(this->GetEntityId(), &AZ::TransformBus::Events::SetLocalTM, transform);
-    }
-
-    void MotorizedJointComponent::ApplyLinVelRigidBodyImpulse(float velocity, float deltaTime)
-    {
-        AZ::Quaternion transform;
-        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetWorldRotationQuaternion);
-        auto force_impulse = transform.TransformVector(m_effortAxis * velocity);
-        Physics::RigidBodyRequestBus::Event(
-            this->GetEntityId(), &Physics::RigidBodyRequests::ApplyLinearImpulse, force_impulse * deltaTime);
-    }
-
-    void MotorizedJointComponent::ApplyLinVelRigidBody(float velocity, float deltaTime)
-    {
-        AZ::Quaternion transform;
-        AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetWorldRotationQuaternion);
-        AZ::Vector3 currentVelocity;
-        auto transformed_velocity_increment = transform.TransformVector(m_effortAxis * velocity);
-        Physics::RigidBodyRequestBus::EventResult(currentVelocity, this->GetEntityId(), &Physics::RigidBodyRequests::GetLinearVelocity);
-        AZ::Vector3 new_velocity = currentVelocity + transformed_velocity_increment;
-        Physics::RigidBodyRequestBus::Event(this->GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, new_velocity);
-    }
-
-    void MotorizedJointComponent::SetSetpoint(float setpoint)
-    {
-        m_setpoint = setpoint;
-    }
-
-    float MotorizedJointComponent::GetSetpoint()
-    {
-        return m_setpoint;
-    }
-
-    float MotorizedJointComponent::GetError()
-    {
-        return m_error;
-    }
-
-    float MotorizedJointComponent::GetCurrentMeasurement()
-    {
-        return m_currentPosition - m_zeroOffset;
-    }
-
-} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/PidMotorControllerComponent.cpp

@@ -0,0 +1,89 @@
+/*
+ * 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 <ROS2/Manipulation/PidMotorControllerComponent.h>
+#include <imgui/imgui.h>
+
+namespace ROS2
+{
+    void PidMotorControllerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto* serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<PidMotorControllerComponent, JointMotorControllerComponent>()
+                ->Field("ZeroOffset", &PidMotorControllerComponent::m_zeroOffset)
+                ->Field("PidPosition", &PidMotorControllerComponent::m_pidPos)
+                ->Version(2);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<PidMotorControllerComponent>("Pid Motor Controller", "")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &PidMotorControllerComponent::m_zeroOffset,
+                        "Zero Offset",
+                        "Allows to change offset of zero to set point")
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &PidMotorControllerComponent::m_pidPos, "Pid Position", "Pid Position");
+            }
+        }
+    }
+
+    void PidMotorControllerComponent::Activate()
+    {
+        m_pidPos.InitializePid();
+        PidMotorControllerRequestBus::Handler::BusConnect(GetEntityId());
+        JointMotorControllerComponent::Activate();
+    }
+
+    void PidMotorControllerComponent::Deactivate()
+    {
+        JointMotorControllerComponent::Deactivate();
+        PidMotorControllerRequestBus::Handler::BusDisconnect();
+    }
+
+    void PidMotorControllerComponent::SetSetpoint(float setpoint)
+    {
+        m_setPoint = setpoint;
+    }
+
+    float PidMotorControllerComponent::GetSetpoint()
+    {
+        return m_setPoint;
+    }
+
+    float PidMotorControllerComponent::GetCurrentMeasurement()
+    {
+        return m_currentPosition - m_zeroOffset;
+    }
+
+    float PidMotorControllerComponent::GetError()
+    {
+        return m_error;
+    }
+
+    float PidMotorControllerComponent::CalculateMotorSpeed([[maybe_unused]] float deltaTime)
+    {
+        const float controlPositionError = (m_setPoint + m_zeroOffset) - m_currentPosition;
+        m_error = controlPositionError;
+
+        const auto deltaTimeNs = aznumeric_cast<uint64_t>(deltaTime * 1.0e9f);
+        return aznumeric_cast<float>(m_pidPos.ComputeCommand(controlPositionError, deltaTimeNs));
+    }
+
+    void PidMotorControllerComponent::DisplayControllerParameters()
+    {
+        AZStd::pair<float, float> limits{ 0.0f, 0.0f };
+        PhysX::JointRequestBus::EventResult(limits, m_jointComponentIdPair, &PhysX::JointRequests::GetLimits);
+
+        ImGui::PushItemWidth(200.0f);
+        ImGui::SliderFloat("SetPoint", &m_setPoint, limits.first + m_zeroOffset, limits.second + m_zeroOffset);
+        ImGui::PopItemWidth();
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -17,9 +17,11 @@
 #include <Lidar/ROS2LidarSensorComponent.h>
 #include <Odometry/ROS2OdometrySensorComponent.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/Manipulation/JointMotorControllerComponent.h>
 #include <ROS2/Manipulation/JointPublisherComponent.h>
 #include <ROS2/Manipulation/ManipulatorControllerComponent.h>
-#include <ROS2/Manipulation/MotorizedJointComponent.h>
+#include <ROS2/Manipulation/ManualMotorControllerComponent.h>
+#include <ROS2/Manipulation/PidMotorControllerComponent.h>
 #include <RobotControl/Controllers/AckermannController/AckermannControlComponent.h>
 #include <RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h>
 #include <RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h>
@@ -30,7 +32,6 @@
 #include <VehicleDynamics/ModelComponents/AckermannModelComponent.h>
 #include <VehicleDynamics/ModelComponents/SkidSteeringModelComponent.h>
 #include <VehicleDynamics/VehicleModelComponent.h>
-
 #include <VehicleDynamics/WheelControllerComponent.h>
 
 namespace ROS2
@@ -49,28 +50,33 @@ namespace ROS2
             // This happens through the [MyComponent]::Reflect() function.
             m_descriptors.insert(
                 m_descriptors.end(),
-                { ROS2SystemComponent::CreateDescriptor(),
-                  LidarRegistrarSystemComponent::CreateDescriptor(),
-                  ROS2RobotImporterSystemComponent::CreateDescriptor(),
-                  ROS2SensorComponent::CreateDescriptor(),
-                  ROS2ImuSensorComponent::CreateDescriptor(),
-                  ROS2GNSSSensorComponent::CreateDescriptor(),
-                  ROS2LidarSensorComponent::CreateDescriptor(),
-                  ROS2OdometrySensorComponent::CreateDescriptor(),
-                  ROS2FrameComponent::CreateDescriptor(),
-                  ROS2RobotControlComponent::CreateDescriptor(),
-                  ROS2CameraSensorComponent::CreateDescriptor(),
-                  AckermannControlComponent::CreateDescriptor(),
-                  RigidBodyTwistControlComponent::CreateDescriptor(),
-                  SkidSteeringControlComponent::CreateDescriptor(),
-                  ROS2SpawnerComponent::CreateDescriptor(),
-                  ROS2SpawnPointComponent::CreateDescriptor(),
-                  VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(),
-                  VehicleDynamics::WheelControllerComponent::CreateDescriptor(),
-                  VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(),
-                  MotorizedJointComponent::CreateDescriptor(),
-                  JointPublisherComponent::CreateDescriptor(),
-                  ManipulatorControllerComponent::CreateDescriptor() });
+                {
+                    ROS2SystemComponent::CreateDescriptor(),
+                    LidarRegistrarSystemComponent::CreateDescriptor(),
+                    ROS2RobotImporterSystemComponent::CreateDescriptor(),
+                    ROS2SensorComponent::CreateDescriptor(),
+                    ROS2ImuSensorComponent::CreateDescriptor(),
+                    ROS2GNSSSensorComponent::CreateDescriptor(),
+                    ROS2LidarSensorComponent::CreateDescriptor(),
+                    ROS2OdometrySensorComponent::CreateDescriptor(),
+                    ROS2FrameComponent::CreateDescriptor(),
+                    ROS2RobotControlComponent::CreateDescriptor(),
+                    ROS2CameraSensorComponent::CreateDescriptor(),
+                    AckermannControlComponent::CreateDescriptor(),
+                    RigidBodyTwistControlComponent::CreateDescriptor(),
+                    SkidSteeringControlComponent::CreateDescriptor(),
+                    ROS2CameraSensorComponent::CreateDescriptor(),
+                    ROS2SpawnerComponent::CreateDescriptor(),
+                    ROS2SpawnPointComponent::CreateDescriptor(),
+                    VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(),
+                    VehicleDynamics::WheelControllerComponent::CreateDescriptor(),
+                    VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(),
+                    JointMotorControllerComponent::CreateDescriptor(),
+                    ManualMotorControllerComponent::CreateDescriptor(),
+                    JointPublisherComponent::CreateDescriptor(),
+                    ManipulatorControllerComponent::CreateDescriptor(),
+                    PidMotorControllerComponent::CreateDescriptor(),
+                });
         }
 
         //! Add required SystemComponents to the SystemEntity.

Gems/ROS2/Code/ros2_files.cmake

@@ -37,9 +37,12 @@ set(FILES
         Source/Lidar/LidarTemplateUtils.h
         Source/Lidar/ROS2LidarSensorComponent.cpp
         Source/Lidar/ROS2LidarSensorComponent.h
-        Source/Manipulation/MotorizedJointComponent.cpp
+        Source/Manipulation/JointMotorControllerComponent.cpp
+        Source/Manipulation/JointMotorControllerConfiguration.cpp
         Source/Manipulation/JointPublisherComponent.cpp
         Source/Manipulation/ManipulatorControllerComponent.cpp
+        Source/Manipulation/ManualMotorControllerComponent.cpp
+        Source/Manipulation/PidMotorControllerComponent.cpp
         Source/Odometry/ROS2OdometrySensorComponent.cpp
         Source/Odometry/ROS2OdometrySensorComponent.h
         Source/RobotControl/Ackermann/AckermannSubscriptionHandler.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -8,10 +8,13 @@ set(FILES
         Include/ROS2/Frame/NamespaceConfiguration.h
         Include/ROS2/Frame/ROS2FrameComponent.h
         Include/ROS2/Frame/ROS2Transform.h
-        Include/ROS2/Manipulation/MotorizedJointBus.h
-        Include/ROS2/Manipulation/MotorizedJointComponent.h
+        Include/ROS2/Manipulation/JointMotorControllerComponent.h
+        Include/ROS2/Manipulation/JointMotorControllerConfiguration.h
         Include/ROS2/Manipulation/JointPublisherComponent.h
         Include/ROS2/Manipulation/ManipulatorControllerComponent.h
+        Include/ROS2/Manipulation/ManualMotorControllerComponent.h
+        Include/ROS2/Manipulation/PidMotorControllerBus.h
+        Include/ROS2/Manipulation/PidMotorControllerComponent.h
         Include/ROS2/RobotControl/ControlConfiguration.h
         Include/ROS2/RobotControl/ControlSubscriptionHandler.h
         Include/ROS2/Lidar/LidarRaycasterBus.h