Add gripper's initial position setting in FingerGripperComponent (#839)

Signed-off-by: Kacper Dąbrowski <[email protected]>

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

@@ -57,7 +57,8 @@ namespace ROS2
                 ->Field("VelocityEpsilon", &FingerGripperComponent::m_velocityEpsilon)
                 ->Field("DistanceEpsilon", &FingerGripperComponent::m_goalTolerance)
                 ->Field("StallTime", &FingerGripperComponent::m_stallTime)
-                ->Version(1);
+                ->Field("InitialPosition", &FingerGripperComponent::m_initialPosition)
+                ->Version(2);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -81,7 +82,12 @@ namespace ROS2
                         AZ::Edit::UIHandlers::Default,
                         &FingerGripperComponent::m_stallTime,
                         "Stall Time",
-                        "The time to wait before considering the gripper as stalled.");
+                        "The time to wait before considering the gripper as stalled.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FingerGripperComponent::m_initialPosition,
+                        "Initial Position",
+                        "The initial position of the gripper in units of the gripper's joints (meters if prismatic, radians if revolute).");
             }
         }
     }
@@ -179,7 +185,7 @@ namespace ROS2
     {
         m_grippingInProgress = false;
         m_cancelled = true;
-        SetPosition(0.0f, AZStd::numeric_limits<float>::infinity());
+        SetPosition(m_initialPosition, AZStd::numeric_limits<float>::infinity());
         return AZ::Success();
     }
 
@@ -268,7 +274,7 @@ namespace ROS2
         {
             m_initialised = true;
             GetFingerJoints();
-            SetPosition(0.0f, AZStd::numeric_limits<float>::infinity());
+            SetPosition(m_initialPosition, AZStd::numeric_limits<float>::infinity());
         }
 
         if (IsGripperVelocity0())

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

@@ -80,5 +80,6 @@ namespace ROS2
         float m_velocityEpsilon{ 0.01f }; //!< The epsilon value used to determine whether the gripper is moving
         float m_goalTolerance{ 0.001f }; //!< The epsilon value used to determine whether the gripper reached it's goal
         float m_stallTime{ 0.1f }; //!< The time in seconds to wait before determining the gripper is stalled
+        float m_initialPosition{ 0.0f }; //!< The initial position of the gripper in units of the gripper's joints
     };
 } // namespace ROS2