Updats to make manipulator work

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

Project/Assets/Importer/apple_kraken.prefab

@@ -21,7 +21,14 @@
             "Component_[17071299421335020589]": {
                 "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
                 "Id": 17071299421335020589,
-                "Parent Entity": ""
+                "Parent Entity": "",
+                "Transform Data": {
+                    "Translate": [
+                        0.0,
+                        -0.09698295593261719,
+                        0.20325708389282227
+                    ]
+                }
             },
             "Component_[17444362140239799283]": {
                 "$type": "EditorEntityIconComponent",
@@ -50,6 +57,571 @@
         }
     },
     "Entities": {
+        "Entity_[24179378887263]": {
+            "Id": "Entity_[24179378887263]",
+            "Name": "Parking",
+            "Components": {
+                "Component_[10745707300597443786]": {
+                    "$type": "EditorBoxShapeComponent",
+                    "Id": 10745707300597443786,
+                    "GameView": true,
+                    "ShapeColor": [
+                        1.0,
+                        1.0,
+                        0.7799954414367676
+                    ],
+                    "BoxShape": {
+                        "Configuration": {
+                            "DrawColor": [
+                                1.0,
+                                1.0,
+                                0.7799954414367676
+                            ],
+                            "Dimensions": [
+                                0.20000000298023224,
+                                0.20000000298023224,
+                                0.20000000298023224
+                            ]
+                        }
+                    }
+                },
+                "Component_[1108233454126463066]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 1108233454126463066
+                },
+                "Component_[11620707770112889449]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 11620707770112889449
+                },
+                "Component_[12082676043514489267]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 12082676043514489267
+                },
+                "Component_[14754045729472486156]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 14754045729472486156,
+                    "Parent Entity": "Entity_[75475350413532]",
+                    "Transform Data": {
+                        "Translate": [
+                            -0.16372275352478027,
+                            -1.1648540496826172,
+                            1.6175075769424438
+                        ],
+                        "Rotate": [
+                            -9.998113632202148,
+                            0.0,
+                            0.0
+                        ]
+                    }
+                },
+                "Component_[15938497882653202694]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 15938497882653202694
+                },
+                "Component_[17417196628389277508]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 17417196628389277508
+                },
+                "Component_[2426001587581061828]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 2426001587581061828
+                },
+                "Component_[9544847962351742226]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 9544847962351742226
+                },
+                "Component_[9949927833027888821]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 9949927833027888821
+                }
+            }
+        },
+        "Entity_[26180833647199]": {
+            "Id": "Entity_[26180833647199]",
+            "Name": "SetPoint",
+            "Components": {
+                "Component_[10017064227239912465]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 10017064227239912465
+                },
+                "Component_[10776392178851480604]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 10776392178851480604
+                },
+                "Component_[13171804081852218964]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 13171804081852218964
+                },
+                "Component_[14547926633655814341]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 14547926633655814341
+                },
+                "Component_[17246665219783638533]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 17246665219783638533,
+                    "Parent Entity": "Entity_[5393191934559]",
+                    "Transform Data": {
+                        "Translate": [
+                            -0.5269217491149902,
+                            -0.42015087604522705,
+                            0.3759497404098511
+                        ],
+                        "Rotate": [
+                            -170.00186157226563,
+                            0.013510109856724739,
+                            0.009348486550152302
+                        ]
+                    }
+                },
+                "Component_[1739952913984360720]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 1739952913984360720
+                },
+                "Component_[1964640831655963167]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 1964640831655963167
+                },
+                "Component_[403012336954165657]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 403012336954165657
+                },
+                "Component_[4100906508750857914]": {
+                    "$type": "EditorSphereShapeComponent",
+                    "Id": 4100906508750857914,
+                    "GameView": true,
+                    "ShapeColor": [
+                        0.0,
+                        0.13333334028720856,
+                        1.0
+                    ],
+                    "SphereShape": {
+                        "Configuration": {
+                            "DrawColor": [
+                                0.0,
+                                0.13333334028720856,
+                                1.0
+                            ],
+                            "Radius": 0.10000000149011612
+                        }
+                    }
+                },
+                "Component_[6841971873666905264]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 6841971873666905264
+                }
+            }
+        },
+        "Entity_[32188681862897]": {
+            "Id": "Entity_[32188681862897]",
+            "Name": "apple1",
+            "Components": {
+                "Component_[10199956753690749060]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 10199956753690749060,
+                    "Parent Entity": "Entity_[75475350413532]",
+                    "Transform Data": {
+                        "Translate": [
+                            0.28836727142333984,
+                            -1.6210765838623047,
+                            0.8644837141036987
+                        ]
+                    }
+                },
+                "Component_[10507503928072039112]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 10507503928072039112
+                },
+                "Component_[11472920189170758401]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 11472920189170758401
+                },
+                "Component_[12016234200720800933]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 12016234200720800933
+                },
+                "Component_[12954789826143694255]": {
+                    "$type": "EditorSphereShapeComponent",
+                    "Id": 12954789826143694255,
+                    "GameView": true,
+                    "SphereShape": {
+                        "Configuration": {
+                            "Radius": 0.05000000074505806
+                        }
+                    }
+                },
+                "Component_[13076152645389442766]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 13076152645389442766
+                },
+                "Component_[15956041670584000149]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 15956041670584000149
+                },
+                "Component_[188500975746846314]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 188500975746846314
+                },
+                "Component_[2908405462460737364]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 2908405462460737364
+                },
+                "Component_[9434610330829914013]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 9434610330829914013
+                }
+            }
+        },
+        "Entity_[32197271797489]": {
+            "Id": "Entity_[32197271797489]",
+            "Name": "apple2",
+            "Components": {
+                "Component_[10199956753690749060]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 10199956753690749060,
+                    "Parent Entity": "Entity_[75475350413532]",
+                    "Transform Data": {
+                        "Translate": [
+                            1.0396854877471924,
+                            -1.6810767650604248,
+                            2.3237173557281494
+                        ]
+                    }
+                },
+                "Component_[10507503928072039112]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 10507503928072039112
+                },
+                "Component_[11472920189170758401]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 11472920189170758401
+                },
+                "Component_[12016234200720800933]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 12016234200720800933
+                },
+                "Component_[12954789826143694255]": {
+                    "$type": "EditorSphereShapeComponent",
+                    "Id": 12954789826143694255,
+                    "GameView": true,
+                    "SphereShape": {
+                        "Configuration": {
+                            "Radius": 0.05000000074505806
+                        }
+                    }
+                },
+                "Component_[13076152645389442766]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 13076152645389442766
+                },
+                "Component_[15956041670584000149]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 15956041670584000149
+                },
+                "Component_[188500975746846314]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 188500975746846314
+                },
+                "Component_[2908405462460737364]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 2908405462460737364
+                },
+                "Component_[9434610330829914013]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 9434610330829914013
+                }
+            }
+        },
+        "Entity_[34036328831583]": {
+            "Id": "Entity_[34036328831583]",
+            "Name": "Manipulator",
+            "Components": {
+                "Component_[11597687927208105160]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 11597687927208105160
+                },
+                "Component_[12023151445129612397]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 12023151445129612397
+                },
+                "Component_[1213690404135196621]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 1213690404135196621
+                },
+                "Component_[12196677571982747821]": {
+                    "$type": "ScriptEditorComponent",
+                    "Id": 12196677571982747821,
+                    "ScriptComponent": {
+                        "Properties": {
+                            "Properties": [
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 3894317203,
+                                    "name": "apple1",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[32188681862897]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 1897382185,
+                                    "name": "apple2",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[32197271797489]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 1822771111,
+                                    "name": "debug",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[26180833647199]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 2566542301,
+                                    "name": "gripper",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[5393191934559]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 4245955024,
+                                    "name": "rest",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[24179378887263]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 3189570453,
+                                    "name": "segment1",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[75440990675164]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 655731247,
+                                    "name": "segment2",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[75385156100316]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 1343396537,
+                                    "name": "segment3",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[75410925904092]"
+                                    }
+                                },
+                                {
+                                    "$type": "AzFramework::ScriptPropertyGenericClass",
+                                    "id": 3463843610,
+                                    "name": "segment4",
+                                    "value": {
+                                        "$type": "EntityId",
+                                        "value": "Entity_[75393746034908]"
+                                    }
+                                }
+                            ]
+                        },
+                        "Script": {
+                            "assetId": {
+                                "guid": "{9231205C-39C1-5A43-91C3-17BCE926A862}",
+                                "subId": 1
+                            },
+                            "assetHint": "gem/scripts/apple_kraken_manipulator.luac"
+                        }
+                    },
+                    "ScriptAsset": {
+                        "assetId": {
+                            "guid": "{9231205C-39C1-5A43-91C3-17BCE926A862}",
+                            "subId": 1
+                        },
+                        "assetHint": "gem/scripts/apple_kraken_manipulator.luac"
+                    }
+                },
+                "Component_[16024861503322456588]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 16024861503322456588
+                },
+                "Component_[1730910515259463933]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 1730910515259463933
+                },
+                "Component_[17378691347384634631]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 17378691347384634631
+                },
+                "Component_[17622739235833867082]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 17622739235833867082,
+                    "Parent Entity": "Entity_[75475350413532]"
+                },
+                "Component_[18423304405568569829]": {
+                    "$type": "GenericComponentWrapper",
+                    "Id": 18423304405568569829,
+                    "m_template": {
+                        "$type": "InputConfigurationComponent",
+                        "Input Event Bindings": {
+                            "assetId": {
+                                "guid": "{7B71C3F3-7E3B-5D0C-B02D-11B655BD5C2E}"
+                            },
+                            "assetHint": "gem/scripts/manipulator_control.inputbindings"
+                        }
+                    }
+                },
+                "Component_[2801781530541130302]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 2801781530541130302,
+                    "DisabledComponents": [
+                        {
+                            "$type": "ScriptEditorComponent",
+                            "Id": 12282947503654674697,
+                            "ScriptComponent": {
+                                "Properties": {
+                                    "Properties": [
+                                        {
+                                            "$type": "AzFramework::ScriptPropertyGenericClass",
+                                            "id": 1822771111,
+                                            "name": "debug",
+                                            "value": {
+                                                "$type": "EntityId",
+                                                "value": "Entity_[26180833647199]"
+                                            }
+                                        },
+                                        {
+                                            "$type": "AzFramework::ScriptPropertyGenericClass",
+                                            "id": 2566542301,
+                                            "name": "gripper",
+                                            "value": {
+                                                "$type": "EntityId",
+                                                "value": "Entity_[5393191934559]"
+                                            }
+                                        },
+                                        {
+                                            "$type": "AzFramework::ScriptPropertyGenericClass",
+                                            "id": 4245955024,
+                                            "name": "rest",
+                                            "value": {
+                                                "$type": "EntityId",
+                                                "value": "Entity_[24179378887263]"
+                                            }
+                                        }
+                                    ]
+                                },
+                                "Script": {
+                                    "assetId": {
+                                        "guid": "{C754714A-D707-572B-BE64-04972182588C}",
+                                        "subId": 1
+                                    },
+                                    "assetHint": "gem/scripts/apple_kraken_manipulator_mock.luac"
+                                }
+                            },
+                            "ScriptAsset": {
+                                "assetId": {
+                                    "guid": "{C754714A-D707-572B-BE64-04972182588C}",
+                                    "subId": 1
+                                },
+                                "assetHint": "gem/scripts/apple_kraken_manipulator_mock.luac"
+                            }
+                        }
+                    ]
+                },
+                "Component_[3570157607065538715]": {
+                    "$type": "GenericComponentWrapper",
+                    "Id": 3570157607065538715,
+                    "m_template": {
+                        "$type": "ApplePickerComponent"
+                    }
+                },
+                "Component_[9474054171365238479]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 9474054171365238479
+                }
+            }
+        },
+        "Entity_[5393191934559]": {
+            "Id": "Entity_[5393191934559]",
+            "Name": "Gripper",
+            "Components": {
+                "Component_[11159106305430731126]": {
+                    "$type": "EditorLockComponent",
+                    "Id": 11159106305430731126
+                },
+                "Component_[12665993018237762079]": {
+                    "$type": "EditorDisabledCompositionComponent",
+                    "Id": 12665993018237762079
+                },
+                "Component_[17015930458439874058]": {
+                    "$type": "EditorPendingCompositionComponent",
+                    "Id": 17015930458439874058
+                },
+                "Component_[1801987919506049247]": {
+                    "$type": "EditorVisibilityComponent",
+                    "Id": 1801987919506049247
+                },
+                "Component_[2869127934271498525]": {
+                    "$type": "EditorEntitySortComponent",
+                    "Id": 2869127934271498525,
+                    "Child Entity Order": [
+                        "Entity_[26180833647199]"
+                    ]
+                },
+                "Component_[4251549409851825295]": {
+                    "$type": "EditorEntityIconComponent",
+                    "Id": 4251549409851825295
+                },
+                "Component_[4303923007737669023]": {
+                    "$type": "EditorInspectorComponent",
+                    "Id": 4303923007737669023
+                },
+                "Component_[703294500506556010]": {
+                    "$type": "EditorSphereShapeComponent",
+                    "Id": 703294500506556010,
+                    "GameView": true,
+                    "DisplayFilled": false,
+                    "SphereShape": {
+                        "Configuration": {
+                            "IsFilled": false,
+                            "Radius": 0.10000000149011612
+                        }
+                    }
+                },
+                "Component_[72200838652418388]": {
+                    "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent",
+                    "Id": 72200838652418388,
+                    "Parent Entity": "Entity_[75393746034908]",
+                    "Transform Data": {
+                        "Translate": [
+                            0.0,
+                            -2.384185791015625e-7,
+                            0.4757688045501709
+                        ],
+                        "Rotate": [
+                            0.0,
+                            90.0,
+                            90.0
+                        ]
+                    }
+                },
+                "Component_[8946861997072969434]": {
+                    "$type": "EditorOnlyEntityComponent",
+                    "Id": 8946861997072969434
+                }
+            }
+        },
         "Entity_[10566975603038]": {
             "Id": "Entity_[10566975603038]",
             "Name": "Reach",
@@ -240,6 +812,7 @@
                     "Id": 1426053021118604394,
                     "Configuration": {
                         "entityId": "",
+                        "Gravity Enabled": false,
                         "Compute Mass": false,
                         "Mass": 10.0,
                         "Compute COM": false,
@@ -382,6 +955,7 @@
                     "$type": "EditorEntitySortComponent",
                     "Id": 12295880520237820639,
                     "Child Entity Order": [
+                        "Entity_[5393191934559]",
                         "Entity_[75432400740572]"
                     ]
                 },
@@ -390,6 +964,7 @@
                     "Id": 14087896415770587582,
                     "Configuration": {
                         "entityId": "",
+                        "Gravity Enabled": false,
                         "Compute Mass": false,
                         "Mass": 5.0,
                         "Compute COM": false,
@@ -801,6 +1376,7 @@
                     "Id": 10581745089606900032,
                     "Configuration": {
                         "entityId": "",
+                        "Gravity Enabled": false,
                         "Compute Mass": false,
                         "Mass": 5.0,
                         "Compute COM": false,
@@ -1564,6 +2140,7 @@
                     "Id": 3237663314328519226,
                     "Configuration": {
                         "entityId": "",
+                        "Gravity Enabled": false,
                         "Compute Mass": false,
                         "Mass": 25.0,
                         "Compute COM": false,
@@ -1581,8 +2158,8 @@
                         ]
                     },
                     "PhysXSpecificConfiguration": {
-                        "SolverPositionIterations": 100,
-                        "SolverVelocityIterations": 10
+                        "SolverPositionIterations": 255,
+                        "SolverVelocityIterations": 255
                     }
                 },
                 "Component_[3506748920400888641]": {
@@ -2238,14 +2815,7 @@
                     "$type": "EditorEntitySortComponent",
                     "Id": 11557544210003172907,
                     "Child Entity Order": [
-                        "Entity_[75453875577052]",
-                        "Entity_[75423810805980]",
-                        "Entity_[75440990675164]",
-                        "Entity_[75436695707868]",
-                        "Entity_[75402335969500]",
-                        "Entity_[75479645380828]",
-                        "Entity_[75419515838684]",
-                        "Entity_[10566975603038]"
+
                     ]
                 },
                 "Component_[11587227330168097343]": {
@@ -2371,8 +2941,8 @@
                         ]
                     },
                     "PhysXSpecificConfiguration": {
-                        "SolverPositionIterations": 40,
-                        "SolverVelocityIterations": 10
+                        "SolverPositionIterations": 255,
+                        "SolverVelocityIterations": 255
                     }
                 },
                 "Component_[2018556428293378569]": {

Project/Gem/Scripts/apple_kraken_manipulator.lua

@@ -0,0 +1,394 @@
+--[[
+    Manipulator control
+--]]
+
+function clamp(value, lower_limit, upper_limit)
+    if lower_limit == upper_limit then
+        return value
+    else
+        if value > upper_limit then
+            return upper_limit
+        elseif value < lower_limit then
+            return lower_limit
+        else
+            return value
+        end
+    end
+end
+
+local PID = {}
+PID.__index = PID
+
+function PID.new(kp, kd, ki, limit_output)
+    local self = setmetatable({}, PID)
+    self._kp = kp
+    self._kd = kd
+    self._ki = ki
+    self._preError = 0
+    self._integral = 0
+    self._limit = limit_output
+    return self
+end
+
+function PID:Reset()
+    self._preError = 0
+    self._integral = 0
+end
+
+function PID:Calculate(dt, err)
+    local pOut = (self._kp * err)
+    self._integral = self._integral + (err * dt)
+    local iOut = (self._ki * self._integral)
+    local deriv = ((err - self._preError) / dt)
+    local dOut = (self._kd * deriv)
+    output = (pOut + iOut + dOut)
+    self._preError = err
+    if self._limit>0.0 then
+        output = clamp(output, -self._limit, self._limit)
+    end
+    return output
+end
+
+local manipulator_control = 
+{
+    Properties =
+    {
+    segment1 = { default = EntityId() },
+    segment2 = { default = EntityId() },
+    segment3 = { default = EntityId() },
+    segment4 = { default = EntityId() },
+
+    gripper = { default = EntityId() },
+    rest = { default = EntityId() },
+    debug = { default = EntityId() },
+
+    apple1 = { default = EntityId() },
+    apple2 = { default = EntityId() },
+    }
+}
+
+State = {
+    IDLE = 0,  --!< Idle state / position, suitable for robot moving around the environment.
+    PREPARED = 10, --!< State and position which are ready for picking tasks.
+    PICKING_BASE = 20, --!< The effector is on its way to pick fruit.
+    PICKING_NOSE = 25, --!< The effector is on its way to pick fruit.
+    RETRIEVING_NOSE = 30, --!< The effector is retrieving a fruit to storage position.
+    RETRIEVING_BASE = 35, --!< The effector is retrieving a fruit to storage position.
+    INVALID = -1, --!< Invalid state. Requires an additional context that could help user understand what happened. @see PickingState.
+}
+
+function manipulator_control:OnActivate()
+
+    ------------------------------------
+    -- Configuration parameters BEGIN --
+
+    -- Maximum velocity in X (in vehicle axis), Y (suction tube), 
+    -- Z (vertical) directions
+    self.max_velocity = Vector3(80.8, 30.0, 80.0) 
+
+    -- To prevent violent reactions right after the simulation starts,
+    -- we're waiting this ammount of seconds till running the controller
+    self.startupWait = 0.5 --[s]
+
+    -- Zero threshold - is used to check if manipulator reached the destination.
+    -- If absolute value of error (target_position - current_position)
+    -- is lower than these values, it's assumed that destination was reached.
+    -- It is defined for each segment separatelly.
+    self.nearZeroThresholds = {0.1, 0.1, 0.1, 0.1} -- [m]
+
+    -- When manipulator moves, it performes X and Z movements first (movement of 
+    -- the base), when destination is reached (based on self.nearZeroThresholds), 
+    -- it starts moving the suction tube ("nose"). Before moving the nose it 
+    -- waits self.baseZeroTimeout to stabilize.
+    self.baseZeroTimeout = 0.1 --[s]
+
+    -- If true, [Numpad 1] and [Numpad 2] keys can be used to select between 
+    -- "apple1" and "apple2" entities
+    self.enableKeyboardInput = true
+
+    -- If true, current manipulaor state will be printed
+    self.printState = true
+
+
+    -- If true, apple will be retreived automatically after reaching. If false,
+    -- Retrieve() function must be called.
+    self.autoRetrieve = true
+
+    -- Configuration parameters END --
+    ------------------------------------
+
+    self.tickBusHandler = TickBus.CreateHandler(self)
+    self.tickBusHandler:Connect()    
+
+    self.InputNotificationBus = InputEventNotificationBus.Connect(self, InputEventNotificationId("manipulator_keyboard_control"))
+
+    -- = {0.0, 0.0, 0.0, 0.0}
+
+    self.pid1 = PID.new(600.0, 50.0, 10.0, self.max_velocity['z']) 
+    self.pid2 = PID.new(600.0, 50.0, 0.0, self.max_velocity['x']) 
+    self.pid3 = PID.new(200.0, 20.0, 0.0, self.max_velocity['y'])
+    self.pid4 = PID.new(200.0, 10.0, 0.0, self.max_velocity['y'])
+    self.gravityThreshold = 0.0
+
+    -- Target translation of each segment. Relative to current position
+    self.internalError = {nil, nil, nil, nil}
+
+    -- Target position (x,y,z) in internal reference
+    self.targetPos = Vector3(nil, nil, nil)
+
+    -- Target
+    self.requestWorldPos = Vector3(nil, nil, nil)
+    self.restPos = Vector3(0.0, -0.1, 0.0)
+
+    self.pickingState = State.PREPARED
+    self.lastPickingState = nil
+
+    self.baseZeroTime = 0.0
+
+    self.noseZeroTimeout = 0.5
+    self.noseZeroTime = 0.0
+end
+
+function manipulator_control:getSegmentPos(entityid)
+    return TransformBus.Event.GetLocalTranslation(entityid)
+end
+
+function manipulator_control:_setSegmentPos(entityid, pid, target_pos, axis1, print_debug)
+
+    local err = target_pos
+    local force = pid:Calculate(self.deltaTime, err)
+
+    local impulse = force * self.deltaTime
+
+    local force_vector = Vector3(0.0, 0.0, impulse)
+
+    local tm = TransformBus.Event.GetWorldTM(entityid)
+
+    force_vector = Transform.TransformVector(tm, force_vector)
+
+    -- If we want to use velocity
+    RigidBodyRequestBus.Event.SetLinearVelocity(entityid, force_vector)   
+    -- If we want to use force impulse
+    --RigidBodyRequestBus.Event.ApplyLinearImpulse(entityid, force_vector)   
+
+    if print_debug then
+        Debug.Log('error: '..string.format("%1.3f",target_pos)..'  impulse: '..string.format("%1.3f",force))
+    end
+end
+
+function manipulator_control:_setPosition()
+    self:_setSegmentPos(self.Properties.segment1, self.pid1, self.internalError[1], 'z', false)
+    self:_setSegmentPos(self.Properties.segment2, self.pid2, self.internalError[2], 'x', false)
+    self:_setSegmentPos(self.Properties.segment3, self.pid3, self.internalError[3], 'y', false)
+    self:_setSegmentPos(self.Properties.segment4, self.pid4, self.internalError[4], 'z', false)
+end
+
+
+function manipulator_control:_getWorldPosition(target_position)
+    local tm = TransformBus.Event.GetWorldTM(self.Properties.gripper)
+    local gripper_pos = Transform.GetTranslation(tm)
+    Transform.Invert(tm)
+
+    local pos = Transform.TransformVector(tm, target_position - gripper_pos)
+
+    TransformBus.Event.SetLocalTranslation(self.Properties.debug, pos) 
+
+    return pos
+end
+
+function manipulator_control:_setInternalState()
+    self.internalError[1] = -self.targetPos['z'] + self.gravityThreshold 
+    self.internalError[2] = self.targetPos['x'] 
+    self.internalError[3] = self.targetPos['y'] 
+    self.internalError[4] = self.targetPos['y']
+end
+
+function manipulator_control:_isNearZero(segment)
+    if math.abs(self.internalError[segment]) < self.nearZeroThresholds[segment] then
+        return true
+    else
+        return false
+    end
+end
+
+
+function manipulator_control:_printDebugInfo()
+    if self.lastPickingState ~= self.pickingState then
+        self.lastPickingState = self.pickingState
+
+        if self.printState then
+            if self.pickingState == State.IDLE then
+                txt = 'IDLE'
+
+            elseif self.pickingState == State.PREPARED then
+                txt = 'PREPARED'
+
+            elseif self.pickingState == State.PICKING_BASE then
+                txt = 'PICKING_BASE'
+
+            elseif self.pickingState == State.PICKING_NOSE then
+                txt = 'PICKING_NOSE'
+
+            elseif self.pickingState == State.RETRIEVING_NOSE then
+                txt = 'RETRIEVING_NOSE'
+
+            elseif self.pickingState == State.RETRIEVING_BASE then
+                txt = 'RETRIEVING_BASE'
+
+            elseif self.pickingState == State.INVALID then
+                txt = 'INVALID'
+            end
+
+            Debug.Log("Changed state to: ["..txt.."]")
+        end
+    end
+end
+
+function manipulator_control:_orchestrator()
+
+    ----------------------------------------------------------
+
+    if self.pickingState == State.INVALID then
+        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
+        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
+    end
+
+
+    ----------------------------------------------------------
+
+    if self.pickingState == State.RETRIEVING_BASE then
+        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
+        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
+        if (self:_isNearZero(1) and self:_isNearZero(2) and self:_isNearZero(3) and self:_isNearZero(4)) then
+            self.pickingState = State.PREPARED
+        end
+    end
+
+    ----------------------------------------------------------
+
+    if self.pickingState == State.RETRIEVING_NOSE then
+
+        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
+        self.targetPos['y'] = self:_getWorldPosition(self.requestWorldPos)['y']
+
+        if (self:_isNearZero(3) and self:_isNearZero(4)) then
+            self.noseZeroTime = self.noseZeroTime + self.deltaTime
+            if self.noseZeroTime > self.noseZeroTimeout then
+                self.noseZeroTime = 0.0
+                self.pickingState = State.RETRIEVING_BASE
+            end
+        end
+    end
+
+    ----------------------------------------------------------
+
+    if self.pickingState == State.PREPARED then
+        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
+        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
+    end
+
+    ----------------------------------------------------------
+
+    if self.pickingState == State.PICKING_NOSE then
+
+        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
+
+        if (self:_isNearZero(3) and self:_isNearZero(4)) then
+            self.noseZeroTime = self.noseZeroTime + self.deltaTime
+            if self.noseZeroTime > self.noseZeroTimeout then
+                self.noseZeroTime = 0.0
+                self.pickingTime = 0.0
+                if self.autoRetrieve then
+                    self:Retrieve()
+                end
+            end
+        end
+    end
+
+    ----------------------------------------------------------
+
+    if self.pickingState == State.PICKING_BASE then
+
+        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
+        self.targetPos['y'] = self.restPos['y']
+
+        if (self:_isNearZero(1) and self:_isNearZero(2)) then
+            self.baseZeroTime = self.baseZeroTime + self.deltaTime
+            if self.baseZeroTime > self.baseZeroTimeout then
+                self.baseZeroTime = 0.0
+                self.pickingState = State.PICKING_NOSE
+            end
+        end
+    end
+
+    ----------------------------------------------------------
+
+    self:_setInternalState()
+    self:_setPosition()
+    self:_printDebugInfo()
+end
+
+function manipulator_control:PickApple(apple_pos)
+    --if self.pickingState == state.PREPARED then
+
+    self.requestWorldPos = apple_pos
+    self.pickingState = State.PICKING_BASE
+
+    --Debug.Log("Picking apple at "..tostring(apple_pos))
+
+    --end
+end
+
+
+function manipulator_control:Retrieve()
+    self.pickingState = State.RETRIEVING_NOSE
+    --Debug.Log("Retracting to "..tostring(rest_pos))
+end
+
+function manipulator_control:getStatus()
+    return self.pickingState
+end
+
+
+
+function manipulator_control:OnPressed(value)
+    -- Keypress actions
+
+    if self.enableKeyboardInput then
+        
+        if value == 1.0 then
+            local currentApple = self.Properties.apple1
+            if currentApple ~= nil then
+                self:PickApple(TransformBus.Event.GetWorldTranslation(currentApple))
+            end
+        end
+
+        if value == 2.0 then
+            local currentApple = self.Properties.apple2
+            if currentApple ~= nil then
+                self:PickApple(TransformBus.Event.GetWorldTranslation(currentApple))
+            end
+        end 
+    end
+    
+end
+
+function manipulator_control:OnDeactivate()
+     -- Deactivation Code
+end
+
+-- This callback is called every frame by the tick bus after this entity activates
+function manipulator_control:OnTick(deltaTime, timePoint)
+
+    self.deltaTime = deltaTime
+
+    if self.Properties.segment1~=nil then
+        if self.startupWait > 0 then
+            self.startupWait = self.startupWait -self.deltaTime
+        else
+            self:_orchestrator()
+        end
+    end
+end
+
+return manipulator_control

Project/Gem/Scripts/manipulator_control.inputbindings

@@ -0,0 +1,69 @@
+<ObjectStream version="3">
+	<Class name="InputEventBindingsAsset" type="{25971C7A-26E2-4D08-A146-2EFCC1C36B0C}">
+		<Class name="InputEventBindings" field="Bindings" version="1" type="{14FFD4A8-AE46-4E23-B45B-6A7C4F787A91}">
+			<Class name="AZStd::vector&lt;InputEventGroup, allocator&gt;" field="Input Event Groups" type="{E7A38039-E414-5322-B384-210475DA7732}">
+				<Class name="InputEventGroup" field="element" version="1" type="{25143B7E-2FEC-4CC5-92FE-270B67E79734}">
+					<Class name="AZStd::string" field="Event Name" value="manipulator_keyboard_control" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+					<Class name="AZStd::vector&lt;InputSubComponent*, allocator&gt;" field="Event Generators" type="{7B0B6F41-794A-5CFF-8275-91A3137E747D}">
+						<Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+							<Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_1" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="float" field="Event Value Multiplier" value="1.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+							<Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+						</Class>
+						<Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+							<Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_2" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="float" field="Event Value Multiplier" value="2.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+							<Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+						</Class>
+						<Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+							<Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_3" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="float" field="Event Value Multiplier" value="3.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+							<Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+						</Class>
+						<Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+							<Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_4" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+							<Class name="float" field="Event Value Multiplier" value="4.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+							<Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+						</Class>
+                        <Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+                            <Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_5" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="float" field="Event Value Multiplier" value="5.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                            <Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                        </Class>
+                        <Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+                            <Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_6" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="float" field="Event Value Multiplier" value="6.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                            <Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                        </Class>
+                        <Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+                            <Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_7" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="float" field="Event Value Multiplier" value="7.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                            <Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                        </Class>
+                        <Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+                            <Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_8" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="float" field="Event Value Multiplier" value="8.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                            <Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                        </Class>
+                        <Class name="InputEventMap" field="element" version="2" type="{A14EA0A3-F053-469D-840E-A70002F51384}">
+                            <Class name="AZStd::string" field="Input Device Type" value="keyboard" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="AZStd::string" field="Input Name" value="keyboard_key_numpad_9" type="{03AAAB3F-5C47-5A66-9EBC-D5FA4DB353C9}"/>
+                            <Class name="float" field="Event Value Multiplier" value="9.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                            <Class name="float" field="Dead Zone" value="0.0000000" type="{EA2C3E90-AFBE-44D4-A90D-FAAF79BAF93D}"/>
+                        </Class>
+					</Class>
+					<Class name="bool" field="Exclude From Release" value="false" type="{A0CA880C-AFE4-43CB-926C-59AC48496112}"/>
+				</Class>
+			</Class>
+		</Class>
+	</Class>
+</ObjectStream>
+

Project/Registry/physxsystemconfiguration.setreg

@@ -4,6 +4,8 @@
             "PhysX": {
                 "PhysXSystemConfiguration": {
                     "OverlapBufferSize": 256,
+                    "MaxTimestep": 0.05000000074505806,
+                    "FixedTimeStep": 0.00800000037997961,
                     "CollisionConfig": {
                         "Layers": {
                             "LayerNames": [