Added to KrakenEffector logic to freze usunsed manipulator

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

Project/Gem/Scripts/apple_kraken_manipulator.lua

@@ -401,15 +401,7 @@ function manipulator_control:OnTick(deltaTime, timePoint)
     self.deltaTime = deltaTime
 
     if self.Properties.segment1~=nil then
-        if self.startupWait > 0 then
-            if self:_getFPS() < self.startapMinFPS then
-                self.startupWait = 0.1
-            else
-                self.startupWait = self.startupWait -self.deltaTime
-            end
-        else
-            self:_orchestrator()
-        end
+        self:_orchestrator()
     end
 end
 

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -15,7 +15,8 @@
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <LmbrCentral/Shape/BoxShapeComponentBus.h>
-
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
 namespace AppleKraken
 {
     namespace DebugStateTransit
@@ -100,11 +101,12 @@ namespace AppleKraken
     {
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<KrakenEffectorComponent, AZ::Component>()->Version(2)->Field(
+            serialize->Class<KrakenEffectorComponent, AZ::Component>()->Version(3)->Field(
                 "ReachEntity", &KrakenEffectorComponent::m_reachEntity)
                 ->Field("ManipulatorEntity", &KrakenEffectorComponent::m_manipulatorEntity)
-                ->Field("AppleProbe", &KrakenEffectorComponent::m_appleProbe);
-
+                ->Field("AppleProbe", &KrakenEffectorComponent::m_appleProbe)
+                ->Field("RootManipulatorFreeze",&KrakenEffectorComponent::m_rootEntityToFreeze)
+                ->Field("BaseLinkToKinematic",&KrakenEffectorComponent::m_baseLinkToKinematic);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -122,11 +124,21 @@ namespace AppleKraken
                         &KrakenEffectorComponent::m_manipulatorEntity,
                         "Entity with manipulator",
                         "The entity that has a component handling events from ManipulatorRequestBus")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &KrakenEffectorComponent::m_appleProbe,
+                        "Entity to probe apples",
+                        "Sucking collider")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &KrakenEffectorComponent::m_rootEntityToFreeze,
+                        "RootManipulatorFreeze",
+                        "RootManipulatorFreeze to freeze during robot movement")
                     ->DataElement(
                             AZ::Edit::UIHandlers::Default,
-                            &KrakenEffectorComponent::m_appleProbe,
-                            "Entity to probe apples",
-                            "Sucking collider")
+                            &KrakenEffectorComponent::m_baseLinkToKinematic,
+                            "BaseLinkToKinematic",
+                            "BaseLinkToKinematic during manipulator movement")
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true);
             }
         }
@@ -196,6 +208,7 @@ namespace AppleKraken
     {
         AZ_TracePrintf("KrakenEffectorComponent", "FinishPicking\n");
         BeginTransitionIfAcceptable(EffectorState::IDLE);
+
     }
 
     PickingState KrakenEffectorComponent::GetEffectorState()
@@ -259,6 +272,7 @@ namespace AppleKraken
 
     void KrakenEffectorComponent::OnEffectorReadyForPicking()
     {
+        LockManipulator(false);
         ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::EffectorReadyForPicking);
     }
 
@@ -280,68 +294,82 @@ namespace AppleKraken
         ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::AppleRetrieved);
     }
 
-    void KrakenEffectorComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
-    {
+    void KrakenEffectorComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) {
         m_currentStateTransitionTime += deltaTime;
-        if (m_effectorState == EffectorState::PICKING && m_currentStateTransitionTime > 5)
-        {
+        if (m_effectorState == EffectorState::PICKING && m_currentStateTransitionTime > 5) {
             AZ_Printf("m_onTriggerHandleBeginHandler", "---------------Failed to retrieve apple--------------------\n");
             BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
         }
 
-        if (m_effectorState == EffectorState::RETRIEVING )
-        {
+        if (m_effectorState == EffectorState::RETRIEVING) {
             int status = -1;
             ManipulatorRequestBus::EventResult(status, m_manipulatorEntity, &ManipulatorRequest::GetStatus);
-            if ( status == 10 )
-            {
+            if (status == 10) {
                 BeginTransitionIfAcceptable(EffectorState::PREPARED);
             }
 
         }
+        if (m_effectorState == EffectorState::IDLE) {
+            LockManipulator(true);
+        }
 
-
-
-        if (m_effectorState == m_effectorTargetState)
-        { // //TODO - nothing to do in stub version
+        if (m_effectorState == m_effectorTargetState) { // //TODO - nothing to do in stub version
             return;
         }
 
         // State transition
         AZ_TracePrintf(
-            "KrakenEffectorComponent", "%s", DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
+                "KrakenEffectorComponent", "%s",
+                DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
         m_currentStateTransitionTime = 0.0f;
 
         // Update state
         auto previousState = m_effectorState;
         m_effectorState = m_effectorTargetState;
 
-        if (previousState == EffectorState::IDLE && m_effectorState == EffectorState::PREPARED)
-        {
+        if (previousState == EffectorState::IDLE && m_effectorState == EffectorState::PREPARED) {
             OnEffectorReadyForPicking();
         }
 
-        if (previousState == EffectorState::PICKING && m_effectorState == EffectorState::RETRIEVING)
-        {
+        if (previousState == EffectorState::PICKING && m_effectorState == EffectorState::RETRIEVING) {
             OnApplePicked();
         }
 
-        if (previousState == EffectorState::RETRIEVING && m_effectorState == EffectorState::PREPARED)
-        {
+        if (previousState == EffectorState::RETRIEVING && m_effectorState == EffectorState::PREPARED) {
             OnAppleRetrieved();
         }
-        if (!m_registeredCallback)
-        {
-            auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
-            auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
-            auto [physicScene, physicBody]  = physicsSystem->FindAttachedBodyHandleFromEntityId(m_appleProbe);
-            if (physicBody != AzPhysics::InvalidSimulatedBodyHandle && physicScene != AzPhysics::InvalidSceneHandle)
-            {
-                AzPhysics::SimulatedBody* simulated_body = sceneInterface->GetSimulatedBodyFromHandle(physicScene, physicBody);
+        if (!m_registeredCallback) {
+            auto *physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+            auto *sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+            auto [physicScene, physicBody] = physicsSystem->FindAttachedBodyHandleFromEntityId(m_appleProbe);
+            if (physicBody != AzPhysics::InvalidSimulatedBodyHandle && physicScene != AzPhysics::InvalidSceneHandle) {
+                AzPhysics::SimulatedBody *simulated_body = sceneInterface->GetSimulatedBodyFromHandle(physicScene,
+                                                                                                      physicBody);
                 simulated_body->RegisterOnTriggerEnterHandler(m_onTriggerHandleBeginHandler);
                 m_registeredCallback = true;
             }
 
         }
     }
+    void KrakenEffectorComponent::LockManipulator(bool locked) {
+        AZStd::vector<AZ::EntityId> descendants;
+        AZ::TransformBus::EventResult(descendants, m_rootEntityToFreeze, &AZ::TransformBus::Events::GetAllDescendants);
+        descendants.push_back(m_rootEntityToFreeze);
+        if (is_manipulator_locked != locked){
+            for (auto &descadant: descendants) {
+                if (locked) {
+                    // Lock manipulator, make base_link not kinematic anymore
+                    AZ_Printf("KrakenEffectorComponent", "Locking : %s\n", descadant.ToString().c_str());
+                    Physics::RigidBodyRequestBus::Event(descadant, &Physics::RigidBodyRequestBus::Events::DisablePhysics);
+                    Physics::RigidBodyRequestBus::Event(m_manipulatorEntity, &Physics::RigidBodyRequestBus::Events::SetKinematic,false);
+
+                } else {
+                    // loose manipulator, make base_link kinematic
+                    Physics::RigidBodyRequestBus::Event(descadant, &Physics::RigidBodyRequestBus::Events::EnablePhysics);
+                    Physics::RigidBodyRequestBus::Event(m_manipulatorEntity, &Physics::RigidBodyRequestBus::Events::SetKinematic,true);
+                }
+            }
+            is_manipulator_locked = locked;
+        }
+    }
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -53,14 +53,20 @@ namespace AppleKraken
         void OnApplePicked();
         void OnAppleRetrieved();
 
+        void LockManipulator(bool locked);
+
         PickAppleTask m_currentTask; //!> valid if RETRIEVING or PICKING
         EffectorState m_effectorState = EffectorState::IDLE;
         EffectorState m_effectorTargetState = EffectorState::IDLE;
         float m_currentStateTransitionTime = 0.0f;
         AZ::EntityId m_reachEntity;
         AZ::EntityId m_manipulatorEntity;
+        AZ::EntityId m_rootEntityToFreeze;
         AZ::EntityId m_appleProbe;
+        AZ::EntityId m_baseLinkToKinematic;
+
         bool m_registeredCallback{false};
+        bool is_manipulator_locked = {false};
         AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler m_onTriggerHandleBeginHandler;
     };
 } // namespace AppleKraken