Manipulator controller improvements

- When the nose is retreiving, the added check if it was retrieved before further actions (it was based on timeout previously)
- When no apple is picked, the controller performs a nose retrieval procedure (it was moving to a new position without retrieving nose)
- Some minor improvements

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

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -105,7 +105,6 @@ namespace AppleKraken
                 ->Field("RootManipulatorFreeze", &KrakenEffectorComponent::m_rootEntityToFreeze)
                 ->Field("BaseLinkToKinematic", &KrakenEffectorComponent::m_baseLinkToKinematic)
                 ->Field("RestEntity", &KrakenEffectorComponent::m_restEntityId)
-                ->Field("RetrieveNoseTime", &KrakenEffectorComponent::m_retrieve_nose_time)
                 ->Field("PickStabilizeTime", &KrakenEffectorComponent::m_stabilize_time)
                 ->Field("MaxPickingTime", &KrakenEffectorComponent::m_maxPickingTime);
 
@@ -142,11 +141,6 @@ namespace AppleKraken
                         &KrakenEffectorComponent::m_restEntityId,
                         "ManipulatorRestPoint",
                         "ManipulatorRestPoint")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::EntityId,
-                        &KrakenEffectorComponent::m_retrieve_nose_time,
-                        "Nose Retrieve Time",
-                        "Nose Retrieve Time")
                     ->DataElement(
                         AZ::Edit::UIHandlers::EntityId,
                         &KrakenEffectorComponent::m_stabilize_time,
@@ -375,7 +369,7 @@ namespace AppleKraken
                   if (m_currentStateTransitionTime > m_maxPickingTime)
                   {
                       AZ_Printf("m_onTriggerHandleBeginHandler", "%s : Failed to retrieve apple--------------------\n", GetEntity()->GetName().c_str());
-                      ApplePickingNotificationBus::Event(this->GetEntityId(),&ApplePickingNotifications::PickingFailed, "Timeout");
+                      BeginTransitionIfAcceptable(EffectorState::RETRIEVING_FAILED);
                   }
               } },
             { EffectorState::PICKING_STABILIZE,
@@ -389,10 +383,24 @@ namespace AppleKraken
             { EffectorState::RETRIEVING_NOSE,
               [this]()
               {
-                  if (m_currentStateTransitionTime > m_retrieve_nose_time)
-                  {
-                      BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
-                  }
+                bool result;
+                EBUS_EVENT_ID_RESULT(result, m_manipulatorEntity, ManipulatorRequestBus, IsNoseRetreived);
+
+                if (result)
+                {
+                    BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
+                }
+              } },
+            { EffectorState::RETRIEVING_FAILED,
+              [this]()
+              {
+                bool result;
+                EBUS_EVENT_ID_RESULT(result, m_manipulatorEntity, ManipulatorRequestBus, IsNoseRetreived);
+
+                if (result)
+                {
+                      BeginTransitionIfAcceptable(EffectorState::PREPARED);
+                }
               } },
             {EffectorState::RETRIEVING,
               [this]() {
@@ -465,6 +473,14 @@ namespace AppleKraken
                 {
                 },
             },
+            {
+                { EffectorState::RETRIEVING_FAILED, EffectorState::PREPARED },
+                [this]()
+                {
+                    ApplePickingNotificationBus::Event(this->GetEntityId(),&ApplePickingNotifications::PickingFailed, "Timeout");
+                },
+            },
+
             {
                 { EffectorState::RETRIEVING_STABILIZE, EffectorState::PREPARED },
                 [this]()
@@ -487,6 +503,13 @@ namespace AppleKraken
                     ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::Retrieve);
                 },
             },
+            {
+                { EffectorState::PICKING, EffectorState::RETRIEVING_FAILED },
+                [this]()
+                {
+                    ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::RetrieveNose);
+                },
+            },
         };
     }
 

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -67,7 +67,6 @@ namespace AppleKraken
         PickAppleTask m_currentTask; //!> valid if RETRIEVING or PICKING
         float m_maxPickingTime{ 5.0f };
         float m_currentStateTransitionTime = 0.0f;
-        float m_retrieve_nose_time{ 1.5f };
         float m_stabilize_time{ 0.5f };
         const float m_maxRetrieveTime{ 3.5f };
 

Project/Gem/Source/ApplePicker/PickingStructs.h

@@ -22,8 +22,9 @@ namespace AppleKraken
         PICKING_STABILIZE = 25, //!< The effector waits vacuum being built up
         RETRIEVING_NOSE = 30, //!< The effector is retrieving a fruit to storage position.
         RETRIEVING = 35, //!< The effector is retrieving a fruit to storage position.
-        RETRIEVING_STABILIZE = 40 //!< The effector is retrieving, wait for apple to drop
-
+        RETRIEVING_STABILIZE = 40, //!< The effector is retrieving, wait for apple to drop
+        RETRIEVING_FAILED = 50 //!< The effector is retrieving after failing to pick the apple.
+ 
     };
 
     //! A task to pick a single apple.

Project/Gem/Source/Manipulator/KrakenManipulatorController.cpp

@@ -52,7 +52,8 @@ namespace AppleKraken
                 ->Field("RestEntity", &ManipulatorController::m_restEntity)
                 ->Field("m_effector", &ManipulatorController::m_effector)
                 ->Field("max_errorXZ", &ManipulatorController::max_errorXZ)
-                ->Field("timeXZsetpointReach", &ManipulatorController::m_timeXZsetpointReach);
+                ->Field("max_errorY", &ManipulatorController::max_errorY)
+                ->Field("timeSetpointReach", &ManipulatorController::m_timeSetpointReach);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -69,11 +70,12 @@ namespace AppleKraken
                     ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_effector, "Effector", "Effector")
                     ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_restEntity, "Rest entity", "Rest Entity")
                     ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::max_errorXZ, "max_errorXZ", "max error XZ to retract nose")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::max_errorY, "max_errorY", "max error Y to retract nose")
                     ->DataElement(
                         AZ::Edit::UIHandlers::EntityId,
-                        &ManipulatorController::m_timeXZsetpointReach,
-                        "XZ SetPoint Reach time",
-                        "XZ SetPoint reach time");
+                        &ManipulatorController::m_timeSetpointReach,
+                        "SetPoint Reach time",
+                        "SetPoint reach time");
             }
         }
     }
@@ -134,19 +136,18 @@ namespace AppleKraken
             }
         }
         // auto - disable nose retrieve only if we reached small error.
-        if (m_noseRetrieved == true && error_x < max_errorXZ && error_z < max_errorXZ)
+        if (m_noseRetrieved == true)
         {
             m_time_XZ_ok += deltaTime;
-        }
-        else
-        {
-            m_time_XZ_ok = 0;
-        }
-
-        if (m_noseRetrieved == true && m_time_XZ_ok > m_timeXZsetpointReach)
-        {
-            AZ_Printf("ManipulatorController", "Nose is sliding out  \n");
-            m_noseRetrieved = false;
+            if (m_time_XZ_ok > m_timeSetpointReach)
+            {
+                if (error_x < max_errorXZ  && error_x > -max_errorXZ && error_z < max_errorXZ && error_z > -max_errorXZ)
+                {
+                    AZ_Printf("ManipulatorController", "Nose is sliding out  \n");
+                    m_noseRetrieved = false;
+                    m_time_XZ_ok = 0;
+                }
+            }
         }
         float setpoint_y = position_in_effector_tf.Dot(m_vectorY);
         if (m_entityY.IsValid())
@@ -157,6 +158,21 @@ namespace AppleKraken
                 if (m_noseRetrieved)
                 {
                     setpoint_y = 0;
+                    m_time_Y_ok += deltaTime;
+                    if (m_time_Y_ok > m_timeSetpointReach)
+                    {
+
+                        auto error_y = getMotorizedJoint(m_entityY)->GetError();
+                        if (error_y < max_errorY && error_y > -max_errorY) 
+                        {
+                            m_noseRetrievingSuccess = true;
+                            m_time_Y_ok = 0.0;
+                        }
+                    }
+                }
+                else
+                {
+                    m_noseRetrievingSuccess = false;
                 }
                 component_y->SetSetpoint(setpoint_y);
             }
@@ -174,7 +190,12 @@ namespace AppleKraken
 
     AZ::Vector3 ManipulatorController::GetPosition()
     {
-        return AZ::Vector3(0);
+        auto currentPosition = AZ::Vector3(
+            getMotorizedJoint(m_entityX)->GetCurrentPosition(),
+            getMotorizedJoint(m_entityY)->GetCurrentPosition(),
+            getMotorizedJoint(m_entityZ)->GetCurrentPosition()
+            );
+        return currentPosition;
     };
 
     void ManipulatorController::Retrieve()
@@ -202,4 +223,9 @@ namespace AppleKraken
         m_time_XZ_ok = 0;
     }
 
+    bool ManipulatorController::IsNoseRetreived()
+    {
+        return m_noseRetrievingSuccess;
+    }
+
 } // namespace AppleKraken

Project/Gem/Source/Manipulator/KrakenManipulatorController.h

@@ -41,12 +41,14 @@ namespace AppleKraken
         void Retrieve() override;
         void RetrieveNose() override;
         int GetStatus() override;
+        bool IsNoseRetreived() override;
 
         void ResetTimer();
 
         AZ::Vector3 m_desiredPosition{0, 0, 0 };
         AZStd::optional<AZ::Vector3> m_desiredApple;
         bool m_noseRetrieved{false };
+        bool m_noseRetrievingSuccess{false};
 
         AZ::Vector3 m_vectorX{1, 0, 0 };
         AZ::Vector3 m_vectorY{0, 1, 0 };
@@ -64,8 +66,10 @@ namespace AppleKraken
         float m_setPointZ{ 0 };
 
         float max_errorXZ{ 0.05 };
-        float m_timeXZsetpointReach{1.5 };
+        float max_errorY{ 0.05 };
+        float m_timeSetpointReach{ 0.2 };
 
-        float m_time_XZ_ok = std::numeric_limits<float>::lowest();
+        float m_time_XZ_ok { 0.0 };
+        float m_time_Y_ok { 0.0 };
     };
 } // namespace AppleKraken

Project/Gem/Source/Manipulator/ManipulatorRequestBus.cpp

@@ -37,6 +37,12 @@ namespace AppleKraken
         CallResult(p, FN_GetStatus);
         return p;
     }
+    bool ManipulatorRequestHandler::IsNoseRetreived()
+    {
+        bool p;
+        CallResult(p, FN_IsNoseRetreived);
+        return p;
+    }
 
 
     void ManipulatorRequestHandler::Reflect(AZ::ReflectContext* context)
@@ -49,6 +55,7 @@ namespace AppleKraken
                 ->Event("GetPosition", &ManipulatorRequestBus::Events::GetPosition)
                 ->Event("Retrieve", &ManipulatorRequestBus::Events::Retrieve)
                 ->Event("GetStatus", &ManipulatorRequestBus::Events::GetStatus)
+                ->Event("IsNoseRetreived", &ManipulatorRequestBus::Events::IsNoseRetreived)
                 ->Event("RetrieveNose", &ManipulatorRequestBus::Events::RetrieveNose);
         }
     }

Project/Gem/Source/Manipulator/ManipulatorRequestBus.h

@@ -25,6 +25,7 @@ namespace AppleKraken
         virtual void Retrieve() = 0;
         virtual void RetrieveNose() = 0;
         virtual int GetStatus() = 0;
+        virtual bool IsNoseRetreived() = 0;
     };
 
     using ManipulatorRequestBus = AZ::EBus<ManipulatorRequest>;
@@ -43,7 +44,8 @@ namespace AppleKraken
             GetPosition,
             Retrieve,
             RetrieveNose,
-            GetStatus);
+            GetStatus,
+            IsNoseRetreived);
 
         virtual void PickApple(const AZ::Vector3 position) override;
 
@@ -55,6 +57,8 @@ namespace AppleKraken
 
         virtual int GetStatus() override;
 
+        virtual bool IsNoseRetreived() override;
+
         static void Reflect(AZ::ReflectContext* context);
     };
 } // namespace AppleKraken