Added an outline for the finger gripper controller code

Co-authored by: Antoni Puch <antoni.puch@robotec.ai>
Signed-off-by: Michał Pełka <michal.pelka@robotec.ai>

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

@@ -78,6 +78,13 @@ namespace ROS2
         //! @note the movement is realized by a specific controller and not instant. The joints will then keep this position.
         virtual AZ::Outcome<void, AZStd::string> MoveJointToPosition(const AZStd::string& jointName, JointPosition position) = 0;
 
+        //! Set max effort of an articulation link by name.
+        //! If the joint is not an articulation link, doesn't do anything
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with effort if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<void, AZStd::string> SetMaxJointEffort(const AZStd::string& jointName, JointEffort maxEffort) = 0;
+
         //! Stop the joints movement in progress. It will keep the position in which it stopped.
         virtual void Stop() = 0;
     };

Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.cpp

@@ -0,0 +1,265 @@
+/*
+* 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 "FingerGripperComponent.h"
+#include "Utils.h"
+
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <imgui/imgui.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+
+namespace ROS2
+{
+    void FingerGripperComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("GripperService"));
+    }
+    void FingerGripperComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("GripperService"));
+    }
+
+    void FingerGripperComponent::Activate()
+    {
+        m_grippingInProgress = false;
+        m_initialised = false;
+        m_cancelled = false;
+        m_ImGuiPosition = 0.0f;
+        m_stallingFor = 0.0f;
+        AZ::TickBus::Handler::BusConnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
+        GripperRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void FingerGripperComponent::Deactivate()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
+        GripperRequestBus::Handler::BusDisconnect(GetEntityId());
+    }
+
+    void FingerGripperComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+
+            serialize->Class<FingerGripperComponent, AZ::Component>()
+                ->Field("Epsilon", &FingerGripperComponent::m_epsilon)
+                ->Field("StallTime", &FingerGripperComponent::m_stallTime)
+                ->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<FingerGripperComponent>("FingerGripperComponent", "Component controlling a finger gripper.")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "FingerGripperComponent")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FingerGripperComponent::m_epsilon,
+                        "Epsilon",
+                        "The maximum velocity to consider the gripper as stalled.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FingerGripperComponent::m_stallTime,
+                        "Stall Time",
+                        "The time to wait before considering the gripper as stalled.");
+            }
+        }
+    }
+
+    AZStd::string GetJointName(AZ::EntityId entityId)
+    {
+        AZ::Entity* entity { nullptr };
+        AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId);
+        AZ_Assert(entity, "Entity %s not found.", entityId.ToString().c_str());
+        if (entity)
+        {
+            ROS2FrameComponent* component = entity->FindComponent<ROS2FrameComponent>();
+            if (component)
+            {
+                return component->GetJointName().GetStringView();
+            }
+        }
+        return AZStd::string();
+    }
+
+    ManipulationJoints& FingerGripperComponent::GetFingerJoints()
+    {
+        m_fingerJoints.clear();
+        m_rootOfArticulation = Utils::GetRootOfArticulation(GetEntityId());
+        AZ_Warning("FingerGripperComponent", m_rootOfArticulation.IsValid(), "Entity %s is not part of an articulation.", GetEntity()->GetName().c_str());
+        ManipulationJoints allJoints;
+        if (m_rootOfArticulation.IsValid())
+        {
+            JointsManipulationRequestBus::EventResult(allJoints, m_rootOfArticulation, &JointsManipulationRequests::GetJoints);
+        }
+        AZStd::vector<AZ::EntityId> descendantIds;
+        AZ::TransformBus::EventResult(descendantIds, GetEntityId(), &AZ::TransformBus::Events::GetAllDescendants);
+
+        for (AZ::EntityId descendant : descendantIds)
+        {
+            AZStd::string jointName = GetJointName(descendant);
+            if (!jointName.empty())
+            {
+                AZ_Printf("FingerGripperComponent", "Adding finger joint %s", jointName.c_str());
+                m_fingerJoints[jointName] = allJoints[jointName];
+            }
+        }
+
+        return m_fingerJoints;
+    }
+
+    void FingerGripperComponent::SetPosition(float position, float maxEffort)
+    {
+        // A check to make this work with moveit
+        if (maxEffort == 0.0f)
+        {
+            maxEffort = AZStd::numeric_limits<float>::infinity();
+        }
+
+        float targetPosition = position;
+        for (auto& [jointName, jointInfo] : m_fingerJoints)
+        {
+            AZ::Outcome<void, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(
+                result, m_rootOfArticulation, &JointsManipulationRequests::MoveJointToPosition, jointName, targetPosition);
+            if (!result.IsSuccess())
+            {
+                AZ_Warning("FingerGripperComponent", result, "Joint move cannot be realized: %s for %s ", result.GetError().c_str(), jointName.c_str());
+            }
+        }
+
+        float oneMaxEffort = maxEffort / m_fingerJoints.size();
+        for (auto& [jointName, jointInfo] : m_fingerJoints)
+        {
+            AZ::Outcome<void, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(
+                result, m_rootOfArticulation, &JointsManipulationRequests::SetMaxJointEffort, jointName, oneMaxEffort);
+            if (!result.IsSuccess())
+            {
+                AZ_Warning("FingerGripperComponent", result, "Setting a max force for a joint cannot be realized: %s for %s ", result.GetError().c_str(), jointName.c_str());
+            }
+        }
+    }
+
+    AZ::Outcome<void, AZStd::string> FingerGripperComponent::GripperCommand(float position, float maxEffort)
+    {
+        m_grippingInProgress = true;
+        m_desiredPosition = position;
+        m_maxEffort = maxEffort;
+        m_stallingFor = 0.0f;
+        m_cancelled = false;
+
+        SetPosition(position, maxEffort);
+
+        return AZ::Success();
+    }
+
+    AZ::Outcome<void, AZStd::string> FingerGripperComponent::CancelGripperCommand()
+    {
+        m_grippingInProgress = false;
+        m_cancelled = true;
+        SetPosition(0.0f, 0.0f);
+        return AZ::Success();
+    }
+
+    bool FingerGripperComponent::HasGripperCommandBeenCancelled() const {
+        return m_cancelled;
+    }
+
+    float FingerGripperComponent::GetGripperPosition() const
+    {
+        float gripperPosition = 0.0f;
+        for (const auto& [jointName, _] : m_fingerJoints)
+        {
+            AZ::Outcome<JointPosition, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(result, m_rootOfArticulation, &JointsManipulationRequests::GetJointPosition, jointName);
+            gripperPosition += result.GetValueOr(0.f);
+        }
+
+        return gripperPosition / m_fingerJoints.size();
+    }
+
+    float FingerGripperComponent::GetGripperEffort() const
+    {
+        float gripperEffort = 0.0f;
+        for (const auto& [jointName, _] : m_fingerJoints)
+        {
+            AZ::Outcome<JointEffort, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(result, m_rootOfArticulation, &JointsManipulationRequests::GetJointEffort, jointName);
+            if (result)
+            {
+                gripperEffort += result.GetValue();
+            }
+        }
+        return gripperEffort;
+    }
+
+    bool FingerGripperComponent::IsGripperVelocity0() const
+    {
+
+        float gripperEffort = 0.0f;
+        for (const auto& [jointName, _] : m_fingerJoints)
+        {
+            AZ::Outcome<JointEffort, AZStd::string> velocityResult;
+            JointsManipulationRequestBus::EventResult(velocityResult, m_rootOfArticulation, &JointsManipulationRequests::GetJointVelocity, jointName);
+            if (velocityResult && AZStd::abs(velocityResult.GetValue()) > m_epsilon) {
+                return false;
+            }
+        }
+        return gripperEffort;
+
+        return true;
+    }
+
+    bool FingerGripperComponent::IsGripperNotMoving() const
+    {
+        return m_stallingFor > 1.0f;
+    }
+
+    bool FingerGripperComponent::HasGripperReachedGoal() const
+    {
+        return !m_grippingInProgress || abs(GetGripperPosition() - m_desiredPosition) < m_epsilon;
+    }
+
+    void FingerGripperComponent::OnImGuiUpdate()
+    {
+        ImGui::Begin("FingerGripperDebugger");
+
+        ImGui::SliderFloat("Target Position", &m_ImGuiPosition, 0.0f, 0.1f);
+
+        if (ImGui::Button("Execute Command"))
+        {
+            GripperCommand(m_ImGuiPosition, AZStd::numeric_limits<float>::infinity());
+        }
+
+        ImGui::End();
+    }
+
+    void FingerGripperComponent::OnTick([[maybe_unused]] float delta, [[maybe_unused]] AZ::ScriptTimePoint timePoint)
+    {
+        if (!m_initialised) {
+            m_initialised = true;
+            GetFingerJoints();
+            SetPosition(0.0f, 0.0f);
+        }
+
+        if (IsGripperVelocity0()) {
+            m_stallingFor += delta;
+        }
+        else {
+            m_stallingFor = 0.0f;
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.h

@@ -0,0 +1,85 @@
+/*
+ * 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/TickBus.h>
+#include <AzFramework/Physics/Common/PhysicsEvents.h>
+#include <ImGuiBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <Utilities/ArticulationsUtilities.h>
+
+namespace ROS2
+{
+    //! This component implements finger gripper functionality.
+    class FingerGripperComponent
+        : public AZ::Component
+        , public GripperRequestBus::Handler
+        , public ImGui::ImGuiUpdateListenerBus::Handler
+        , public AZ::TickBus::Handler
+    {
+    public:
+        AZ_COMPONENT(FingerGripperComponent, "{ae5f8ec2-26ee-11ee-be56-0242ac120002}", AZ::Component);
+        FingerGripperComponent() = default;
+        ~FingerGripperComponent() = default;
+
+        // AZ::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:
+        // GripperRequestBus::Handler overrides...
+        AZ::Outcome<void, AZStd::string> GripperCommand(float position, float maxEffort) override;
+        AZ::Outcome<void, AZStd::string> CancelGripperCommand() override;
+        bool HasGripperCommandBeenCancelled() const override;
+
+        // Sum of all joint positions
+        float GetGripperPosition() const override;
+        // Sum of all efforts exerted by fingers
+        float GetGripperEffort() const override;
+        // Non-articulation fingers return 0 effort!
+        bool IsGripperNotMoving() const override;
+        // Doesn't check if the max force is applied, only checks speed
+        bool HasGripperReachedGoal() const override;
+
+        // ImGui::ImGuiUpdateListenerBus::Handler overrides...
+        void OnImGuiUpdate() override;
+
+        // AZ::TickBus::Handler overrides...
+        void OnTick(float delta, AZ::ScriptTimePoint timePoint) override;
+
+        ManipulationJoints& GetFingerJoints();
+
+        AZ::EntityId m_rootOfArticulation; //!< The root of the articulation chain
+
+        float GetDefaultPosition();
+        void SetPosition(float position, float maxEffort);
+        bool IsGripperVelocity0() const;
+        void PublishFeedback() const;
+
+        ManipulationJoints m_fingerJoints;
+        bool m_grippingInProgress{ false };
+        bool m_cancelled{ false };
+        bool m_initialised{ false };
+        float m_desiredPosition{ false };
+        float m_maxEffort{ 0.f };
+        float m_stallingFor{ 0.f };
+        float m_ImGuiPosition{ 0.1f };
+
+        float m_epsilon{ 0.1f }; //!< The epsilon value used to determine if the gripper is moving or not
+        float m_stallTime{ 0.1f }; //!< The time in seconds to wait before determining if the gripper is stalled
+
+    };
+} // namespace ROS2