New manipulator design

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -8,7 +8,7 @@
 
 #include "KrakenEffectorComponent.h"
 #include "ApplePickingNotifications.h"
-#include "ManipulatorRequestBus.h"
+#include "Manipulator/ManipulatorRequestBus.h"
 #include "PickingStructs.h"
 #include <AzCore/Component/TransformBus.h>
 #include <AzCore/Serialization/EditContext.h>
@@ -17,14 +17,25 @@
 #include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
 #include <AzFramework/Physics/RigidBodyBus.h>
 #include <LmbrCentral/Shape/BoxShapeComponentBus.h>
+
 namespace AppleKraken
 {
     namespace DebugStateTransit
     {
+        static const AZStd::unordered_map<EffectorState, AZStd::string> kMapToString{ { EffectorState::INVALID, "INVALID" },
+                                                                                { EffectorState::IDLE, "IDLE" },
+                                                                                { EffectorState::PREPARED, "PREPARED" },
+                                                                                { EffectorState::PICKING, "PICKING" },
+                                                                                { EffectorState::PICKING_STABILIZE, "PICKING_STABILIZE" },
+                                                                                { EffectorState::RETRIEVING, "RETRIEVING" },
+                                                                                { EffectorState::RETRIEVING_NOSE, "RETRIEVING_NOSE" },
+                                                                                { EffectorState::RETRIEVING_STABILIZE,
+                                                                                  "RETRIEVING_STABILIZE" } };
+
         // TODO - this is a debug space for a stub implementation. Proper: a state transition machine with lambdas.
         AZStd::string StateTransitionString(EffectorState current, EffectorState next)
         {
-            return AZStd::string::format("state transition %d -> %d\n", static_cast<int>(current), static_cast<int>(next));
+            return AZStd::string::format("state transition %s -> %s\n", kMapToString.at(current).c_str(), kMapToString.at(next).c_str());
         }
     } // namespace DebugStateTransit
 
@@ -44,6 +55,10 @@ namespace AppleKraken
                 const AZ::EntityId& e1 = event.m_otherBody->GetEntityId();
                 const AZ::EntityId& e2 = event.m_triggerBody->GetEntityId();
                 [[maybe_unused]] const AZ::EntityId& collideToEntityId = m_appleProbe == e1 ? e2 : e1;
+                //                AZStd::string entity_name;
+                //                AZ::ComponentApplicationBus::BroadcastResult(entity_name,
+                //                &AZ::ComponentApplicationRequests::GetEntityName, collideToEntityId);
+                //                AZ_Printf("m_onTriggerHandleBeginHandler", "Collission %s\n", entity_name.c_str());
                 // AzPhysics::SimulatedBody* collideToEntityId = this->GetEntityId() == e1 ?  event.m_triggerBody : event.m_otherBody;}
                 if (m_currentTask.m_appleEntityId == collideToEntityId)
                 {
@@ -52,7 +67,16 @@ namespace AppleKraken
                     if (m_effectorState == EffectorState::PICKING)
                     {
                         // start picking the apple
-                        BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
+                        BeginTransitionIfAcceptable(EffectorState::PICKING_STABILIZE);
+                    }
+                }
+                if (m_restEntityId == collideToEntityId)
+                {
+                    AZ_Printf("m_onTriggerHandleBeginHandler", "=================m_onTriggerHandle to Rest!====================");
+                    if (m_effectorState == EffectorState::RETRIEVING)
+                    {
+                        // start picking the apple
+                        BeginTransitionIfAcceptable(EffectorState::RETRIEVING_STABILIZE);
                     }
                 }
             });
@@ -74,7 +98,11 @@ namespace AppleKraken
                 ->Field("ManipulatorEntity", &KrakenEffectorComponent::m_manipulatorEntity)
                 ->Field("AppleProbe", &KrakenEffectorComponent::m_appleProbe)
                 ->Field("RootManipulatorFreeze", &KrakenEffectorComponent::m_rootEntityToFreeze)
-                ->Field("BaseLinkToKinematic", &KrakenEffectorComponent::m_baseLinkToKinematic);
+                ->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);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -104,6 +132,23 @@ namespace AppleKraken
                         &KrakenEffectorComponent::m_baseLinkToKinematic,
                         "BaseLinkToKinematic",
                         "BaseLinkToKinematic during manipulator movement")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &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,
+                        "PickStabilizeTime",
+                        "PickStabilizeTime")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId, &KrakenEffectorComponent::m_maxPickingTime, "MaxPickingTime", "MaxPickingTime")
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true);
             }
         }
@@ -239,13 +284,13 @@ namespace AppleKraken
                     // 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);
+                    Physics::RigidBodyRequestBus::Event(m_baseLinkToKinematic, &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);
+                    Physics::RigidBodyRequestBus::Event(m_baseLinkToKinematic, &Physics::RigidBodyRequestBus::Events::SetKinematic, true);
                 }
             }
             is_manipulator_locked = locked;
@@ -319,16 +364,35 @@ namespace AppleKraken
                       ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::PickingFailed, "Timeout");
                   }
               } },
+            { EffectorState::PICKING_STABILIZE,
+              [this]()
+              {
+                  if (m_currentStateTransitionTime > m_stabilize_time)
+                  {
+                      BeginTransitionIfAcceptable(EffectorState::RETRIEVING_NOSE);
+                  }
+              } },
+            { EffectorState::RETRIEVING_NOSE,
+              [this]()
+              {
+                  if (m_currentStateTransitionTime > m_retrieve_nose_time)
+                  {
+                      BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
+                  }
+              } },
             { EffectorState::RETRIEVING,
+              []()
+              {
+              } },
+            { EffectorState::RETRIEVING_STABILIZE,
               [this]()
               {
-                  int status = -1;
-                  ManipulatorRequestBus::EventResult(status, m_manipulatorEntity, &ManipulatorRequest::GetStatus);
-                  if (status == 10)
+                  if (m_currentStateTransitionTime > m_stabilize_time)
                   {
                       BeginTransitionIfAcceptable(EffectorState::PREPARED);
                   }
-              } },
+              } }
+
         };
 
         m_stateProperties.m_allowedTransitions = {
@@ -347,7 +411,13 @@ namespace AppleKraken
                 },
             },
             {
-                { EffectorState::PICKING, EffectorState::RETRIEVING },
+                { EffectorState::PICKING, EffectorState::PICKING_STABILIZE },
+                []()
+                {
+                },
+            },
+            {
+                { EffectorState::PICKING_STABILIZE, EffectorState::RETRIEVING_NOSE },
                 [this]()
                 {
                     if (!m_currentTask.IsValid())
@@ -355,16 +425,30 @@ namespace AppleKraken
                         AZ_Error("KrakenEffectorComponent", true, "No valid task for current picking!");
                         return;
                     }
+                    ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::RetrieveNose);
+                },
+            },
+            {
+                { EffectorState::RETRIEVING_NOSE, EffectorState::RETRIEVING },
+                [this]()
+                {
                     ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::Retrieve);
                 },
             },
             {
-                { EffectorState::RETRIEVING, EffectorState::PREPARED },
+                { EffectorState::RETRIEVING, EffectorState::RETRIEVING_STABILIZE },
+                []()
+                {
+                },
+            },
+            {
+                { EffectorState::RETRIEVING_STABILIZE, EffectorState::PREPARED },
                 []()
                 {
                     ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::AppleRetrieved);
                 },
             },
+
             {
                 { EffectorState::PREPARED, EffectorState::IDLE },
                 []()

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -7,8 +7,8 @@
  */
 #pragma once
 
+#include "../Manipulator/ManipulatorRequestBus.h"
 #include "ApplePickingRequests.h"
-#include "ManipulatorRequestBus.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
 #include <AzCore/EBus/EBus.h>
@@ -65,8 +65,11 @@ namespace AppleKraken
         const AZStd::function<void()>& GetCurrentStateAction() const;
 
         PickAppleTask m_currentTask; //!> valid if RETRIEVING or PICKING
-        static constexpr float m_maxPickingTime = 5.0f;
+        float m_maxPickingTime{ 5.0f };
         float m_currentStateTransitionTime = 0.0f;
+        float m_retrieve_nose_time{ 1.5f };
+        float m_stabilize_time{ 0.5f };
+
         EffectorState m_effectorState = EffectorState::IDLE;
         EffectorState m_effectorTargetState = EffectorState::IDLE;
         EffectorStateProperties m_stateProperties;
@@ -75,6 +78,7 @@ namespace AppleKraken
         AZ::EntityId m_rootEntityToFreeze;
         AZ::EntityId m_appleProbe;
         AZ::EntityId m_baseLinkToKinematic;
+        AZ::EntityId m_restEntityId;
 
         bool m_registeredCallback{ false };
         bool is_manipulator_locked = { false };

Project/Gem/Source/ApplePicker/PickingStructs.h

@@ -19,7 +19,11 @@ namespace AppleKraken
         IDLE = 0, //!< Idle state / position, suitable for robot moving around the environment.
         PREPARED = 10, //!< State and position which are ready for picking tasks.
         PICKING = 20, //!< The effector is on its way to pick fruit.
-        RETRIEVING = 30 //!< The effector is retrieving a fruit to storage position.
+        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
+
     };
 
     //! A task to pick a single apple.

Project/Gem/Source/Manipulator/KrakenManipulatorController.cpp

@@ -0,0 +1,205 @@
+/*
+ * 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 "KrakenManipulatorController.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <AzToolsFramework/UI/PropertyEditor/PropertyEditorAPI.h>
+
+namespace AppleKraken
+{
+    ROS2::MotorizedJoint* ManipulatorController::getMotorizedJoint(const AZ::EntityId& entityWithMotJoint)
+    {
+        AZ_Assert(entityWithMotJoint.IsValid(), "Entity Invalid");
+        AZ::Entity* e_ptr{ nullptr };
+        AZ::ComponentApplicationBus::BroadcastResult(e_ptr, &AZ::ComponentApplicationRequests::FindEntity, entityWithMotJoint);
+        AZ_Assert(e_ptr, "No such entity");
+        ROS2::MotorizedJoint* component = e_ptr->FindComponent<ROS2::MotorizedJoint>();
+        //        AZ_Assert(component, "No  ROS2::MotorizedJoint in %s", e_ptr->GetName().c_str());
+        return component;
+    }
+
+    void ManipulatorController::Activate()
+    {
+        ManipulatorRequestBus::Handler::BusConnect(GetEntityId());
+        AZ::TickBus::Handler::BusConnect();
+        initialized = false;
+    }
+
+    void ManipulatorController::Deactivate()
+    {
+        ManipulatorRequestBus::Handler::BusDisconnect(GetEntityId());
+        AZ::TickBus::Handler::BusDisconnect();
+    }
+
+    void ManipulatorController::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ManipulatorController, AZ::Component>()
+                ->Version(2)
+                ->Field("ManipulatorEntityX", &ManipulatorController::m_entityX)
+                ->Field("ManipulatorEntityY", &ManipulatorController::m_entityY)
+                ->Field("ManipulatorEntityZ", &ManipulatorController::m_entityZ)
+                ->Field("ManipulatorVecX", &ManipulatorController::m_vectorX)
+                ->Field("ManipulatorVecY", &ManipulatorController::m_vectorY)
+                ->Field("ManipulatorVecZ", &ManipulatorController::m_vectorZ)
+                ->Field("RestEntity", &ManipulatorController::m_restEntity)
+                ->Field("m_effector", &ManipulatorController::m_effector)
+                ->Field("max_errorXZ", &ManipulatorController::max_errorXZ)
+                ->Field("timeXZsetpointReach", &ManipulatorController::m_timeXZsetpointReach);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<ManipulatorController>("ManipulatorController", "ManipulatorController")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "ManipulatorController")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "AppleKraken")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_entityX, "m_entityX", "m_entityX")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_entityY, "m_entityY", "m_entityY")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_entityZ, "m_entity_z1", "m_entityZ")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_vectorX, "vx", "vx")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_vectorY, "vy", "vy")
+                    ->DataElement(AZ::Edit::UIHandlers::EntityId, &ManipulatorController::m_vectorZ, "vz", "vz")
+                    ->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::m_timeXZsetpointReach,
+                        "XZ SetPoint Reach time",
+                        "XZ SetPoint reach time");
+            }
+        }
+    }
+
+    void ManipulatorController::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (!initialized)
+        {
+            // get offset to effector
+            AZ::Transform transformBaseLink;
+            AZ::TransformBus::EventResult(transformBaseLink, GetEntityId(), &AZ::TransformBus::Events::GetWorldTM);
+            AZ::Transform transformEffector;
+            AZ::TransformBus::EventResult(transformEffector, m_effector, &AZ::TransformBus::Events::GetWorldTM);
+            m_transform_base_link_to_effector = transformBaseLink.GetInverse() * transformEffector;
+
+            initialized = true;
+            return;
+        }
+
+        if (m_desiredApple)
+        {
+            m_desiredPosition = *m_desiredApple;
+        }
+        else
+        {
+            AZ::TransformBus::EventResult(m_desiredPosition, m_restEntity, &AZ::TransformBus::Events::GetWorldTranslation);
+        }
+        // apple is given in the World's coordinate system
+        AZ::Transform current_base_link;
+        AZ::TransformBus::EventResult(current_base_link, GetEntityId(), &AZ::TransformBus::Events::GetWorldTM);
+
+        AZ::Vector3 position_in_baselink_tf = current_base_link.GetInverse().TransformPoint(m_desiredPosition);
+        AZ::Vector3 position_in_effector_tf = m_transform_base_link_to_effector.GetInverse().TransformPoint(position_in_baselink_tf);
+
+        float setpoint_x = position_in_effector_tf.Dot(m_vectorX);
+        float setpoint_z = position_in_effector_tf.Dot(m_vectorZ);
+
+        float error_x = std::numeric_limits<float>::max();
+        float error_z = std::numeric_limits<float>::max();
+
+        if (m_entityX.IsValid())
+        {
+            auto component_x = getMotorizedJoint(m_entityX);
+            if (component_x)
+            {
+                component_x->SetSetpoint(setpoint_x);
+                error_x = component_x->GetError();
+            }
+        }
+
+        if (m_entityZ.IsValid())
+        {
+            auto component_z = getMotorizedJoint(m_entityZ);
+            if (component_z)
+            {
+                component_z->SetSetpoint(setpoint_z);
+                error_z = component_z->GetError();
+            }
+        }
+        // auto - disable nose retrieve only if we reached small error.
+        if (m_noseRetrieved == true && error_x < max_errorXZ && error_z < max_errorXZ)
+        {
+            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;
+        }
+        float setpoint_y = position_in_effector_tf.Dot(m_vectorY);
+        if (m_entityY.IsValid())
+        {
+            auto component_y = getMotorizedJoint(m_entityY);
+            if (component_y)
+            {
+                if (m_noseRetrieved)
+                {
+                    setpoint_y = 0;
+                }
+                component_y->SetSetpoint(setpoint_y);
+            }
+        }
+    }
+
+    void ManipulatorController::PickApple(const AZ::Vector3 position)
+    {
+        m_noseRetrieved = true;
+        m_time_XZ_ok = 0;
+        AZ_Printf("ManipulatorController", "PickApple\n");
+        ResetTimer();
+        m_desiredApple = position;
+    };
+
+    AZ::Vector3 ManipulatorController::GetPosition()
+    {
+        return AZ::Vector3(0);
+    };
+
+    void ManipulatorController::Retrieve()
+    {
+        AZ_Printf("ManipulatorController", "Retrieve\n");
+        m_time_XZ_ok = std::numeric_limits<float>::lowest();
+        m_noseRetrieved = true;
+        m_desiredApple.reset();
+    };
+    void ManipulatorController::RetrieveNose()
+    {
+        AZ_Printf("ManipulatorController", "RetrieveNose\n");
+        m_time_XZ_ok = std::numeric_limits<float>::lowest();
+        m_noseRetrieved = true;
+    }
+
+    int ManipulatorController::GetStatus()
+    {
+        return 0;
+    };
+
+    void ManipulatorController::ResetTimer()
+    {
+        AZ_Printf("ManipulatorController", "Timer is RESET!\n");
+        m_time_XZ_ok = 0;
+    }
+
+} // namespace AppleKraken

Project/Gem/Source/Manipulator/KrakenManipulatorController.h

@@ -0,0 +1,71 @@
+/*
+ * 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 "ManipulatorRequestBus.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzFramework/AzFrameworkModule.h>
+#include <AzFramework/Spawnable/SpawnableEntitiesInterface.h>
+#include <ROS2/Manipulator/MotorizedJoint.h>
+
+namespace AppleKraken
+{
+    //! Component responsible for storing counters of apples gathered by Kraken.
+    class ManipulatorController
+        : public AZ::Component
+        , public ManipulatorRequestBus ::Handler
+        , public AZ::TickBus::Handler
+    {
+    public:
+        AZ_COMPONENT(ManipulatorController, "{1C292F1D-F050-42EB-81C1-F6F83C4929F4}", AZ::Component, ManipulatorRequestBus::Handler);
+
+        // AZ::Component interface implementation.
+        ManipulatorController() = default;
+        ~ManipulatorController() = default;
+        void Activate() override;
+        void Deactivate() override;
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        bool initialized{ false };
+        ROS2::MotorizedJoint* getMotorizedJoint(const AZ::EntityId& entityWithMotJoint);
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+        void PickApple(const AZ::Vector3 position) override;
+        AZ::Vector3 GetPosition() override;
+        void Retrieve() override;
+        void RetrieveNose() override;
+        int GetStatus() override;
+
+        void ResetTimer();
+
+        AZ::Vector3 m_desiredPosition{0, 0, 0 };
+        AZStd::optional<AZ::Vector3> m_desiredApple;
+        bool m_noseRetrieved{false };
+
+        AZ::Vector3 m_vectorX{1, 0, 0 };
+        AZ::Vector3 m_vectorY{0, 1, 0 };
+        AZ::Vector3 m_vectorZ{0, 0, 1 };
+
+        AZ::EntityId m_entityX;
+        AZ::EntityId m_entityY;
+        AZ::EntityId m_entityZ;
+        AZ::EntityId m_effector;
+        AZ::EntityId m_restEntity;
+
+        AZ::Transform m_transform_base_link_to_effector;
+        float m_setPointX{ 0 };
+        float m_setPointY{ 0 };
+        float m_setPointZ{ 0 };
+
+        float max_errorXZ{ 0.05 };
+        float m_timeXZsetpointReach{1.5 };
+
+        float m_time_XZ_ok = std::numeric_limits<float>::lowest();
+    };
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/ManipulatorRequestBus.cpp → Project/Gem/Source/Manipulator/ManipulatorRequestBus.cpp

@@ -27,6 +27,10 @@ namespace AppleKraken
         Call(FN_Retrieve);
     }
 
+    void ManipulatorRequestHandler::RetrieveNose()
+    {
+        Call(FN_RetrieveNose);
+    }
     int ManipulatorRequestHandler::GetStatus()
     {
         int p;
@@ -34,6 +38,7 @@ namespace AppleKraken
         return p;
     }
 
+
     void ManipulatorRequestHandler::Reflect(AZ::ReflectContext* context)
     {
         if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
@@ -43,7 +48,8 @@ namespace AppleKraken
                 ->Event("PickApple", &ManipulatorRequestBus::Events::PickApple)
                 ->Event("GetPosition", &ManipulatorRequestBus::Events::GetPosition)
                 ->Event("Retrieve", &ManipulatorRequestBus::Events::Retrieve)
-                ->Event("GetStatus", &ManipulatorRequestBus::Events::GetStatus);
+                ->Event("GetStatus", &ManipulatorRequestBus::Events::GetStatus)
+                ->Event("RetrieveNose", &ManipulatorRequestBus::Events::RetrieveNose);
         }
     }
 

Project/Gem/Source/ApplePicker/ManipulatorRequestBus.h → Project/Gem/Source/Manipulator/ManipulatorRequestBus.h

@@ -23,6 +23,7 @@ namespace AppleKraken
         virtual void PickApple(const AZ::Vector3 position) = 0;
         virtual AZ::Vector3 GetPosition() = 0;
         virtual void Retrieve() = 0;
+        virtual void RetrieveNose() = 0;
         virtual int GetStatus() = 0;
     };
 
@@ -41,6 +42,7 @@ namespace AppleKraken
             PickApple,
             GetPosition,
             Retrieve,
+            RetrieveNose,
             GetStatus);
 
         virtual void PickApple(const AZ::Vector3 position) override;
@@ -49,6 +51,8 @@ namespace AppleKraken
 
         virtual void Retrieve() override;
 
+        virtual void RetrieveNose() override;
+
         virtual int GetStatus() override;
 
         static void Reflect(AZ::ReflectContext* context);

Project/Gem/Source/ROSConDemoModule.cpp

@@ -11,6 +11,7 @@
 #include "ApplePicker/KrakenEffectorComponent.h"
 #include "DemoStatistics/DemoStatisticsComponent.h"
 #include "FruitStorage/FruitStorageComponent.h"
+#include "Manipulator/KrakenManipulatorController.h"
 #include "ROSConDemoSystemComponent.h"
 #include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/Module/Module.h>
@@ -33,7 +34,8 @@ namespace ROSConDemo
                   AppleKraken::GatheringRowComponent::CreateDescriptor(),
                   AppleKraken::KrakenEffectorComponent::CreateDescriptor(),
                   AppleKraken::FruitStorageComponent::CreateDescriptor(),
-                  AppleKraken::DemoStatisticsComponent::CreateDescriptor() });
+                  AppleKraken::DemoStatisticsComponent::CreateDescriptor(),
+                  AppleKraken::ManipulatorController::CreateDescriptor() });
         }
 
         AZ::ComponentTypeList GetRequiredSystemComponents() const override

Project/Gem/roscondemo_files.cmake

@@ -20,8 +20,10 @@ set(FILES
         Source/FruitStorage/FruitStorageComponent.cpp
         Source/FruitStorage/FruitStorageComponent.h
         Source/FruitStorage/FruitStorageBus.h
-        Source/ApplePicker/ManipulatorRequestBus.cpp
-        Source/ApplePicker/ManipulatorRequestBus.h
+        Source/Manipulator/KrakenManipulatorController.cpp
+        Source/Manipulator/KrakenManipulatorController.h
+        Source/Manipulator/ManipulatorRequestBus.cpp
+        Source/Manipulator/ManipulatorRequestBus.h
         Source/ROSConDemoSystemComponent.cpp
         Source/ROSConDemoSystemComponent.h
         enabled_gems.cmake