diff --git a/Project/Levels/RoboticManipulationBenchmark/RoboticManipulationBenchmark.prefab b/Project/Levels/RoboticManipulationBenchmark/RoboticManipulationBenchmark.prefab index b043d2d..7847ad9 100755 --- a/Project/Levels/RoboticManipulationBenchmark/RoboticManipulationBenchmark.prefab +++ b/Project/Levels/RoboticManipulationBenchmark/RoboticManipulationBenchmark.prefab @@ -1143,21 +1143,6 @@ "path": "/Entities/Entity_[743054958320]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", "value": "assets/urdfimporter/1440394708_panda/link5.dae.azmodel" }, - { - "op": "replace", - "path": "/Entities/Entity_[760234827504]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/AssetScale/0", - "value": 3.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[760234827504]/Components/EditorMeshColliderComponent/ShapeConfiguration/HasNonUniformScale", - "value": true - }, - { - "op": "replace", - "path": "/Entities/Entity_[760234827504]/Components/EditorMeshColliderComponent/HasNonUniformScale", - "value": true - }, { "op": "replace", "path": "/Entities/Entity_[764529794800]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", @@ -1173,11 +1158,6 @@ "path": "/Entities/Entity_[777414696688]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", "value": "assets/urdfimporter/1440394708_panda/link3.dae.azmodel" }, - { - "op": "replace", - "path": "/Entities/Entity_[781709663984]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", - "value": 0.05999999865889549 - }, { "op": "replace", "path": "/Entities/Entity_[790299598576]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", @@ -2103,57 +2083,12 @@ "IsRuntimeActive": true } }, - { - "op": "replace", - "path": "/Entities/Entity_[747349925616]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", - "value": "panda_link2" - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/0/1", - "value": 1.5709999799728394 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/1/1", - "value": 1.5709999799728394 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/2/1", - "value": -1.5709999799728394 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/3/1", - "value": -2.450000047683716 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/4/1", - "value": 1.5709999799728394 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/6/1", - "value": -0.10000000149011612 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/7/1", - "value": 0.019999999552965164 - }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/8/1", - "value": 0.019999999552965164 - }, { "op": "add", - "path": "/Entities/Entity_[807479467760]/Components/EditorColliderComponent_2", + "path": "/Entities/Entity_[755939860208]/Components/EditorDisabledCompositionComponent/DisabledComponents/EditorColliderComponent", "value": { "$type": "EditorColliderComponent", - "Id": 15762480907708745507, + "Id": 13507121597919349583, "ColliderConfiguration": { "CollisionLayer": { "Index": 0 @@ -2169,7 +2104,7 @@ "Exclusive": true, "Position": [ 0.0, - 0.004999999888241291, + -0.004999999888241291, 0.05000000074505806 ], "Rotation": [ @@ -2285,37 +2220,12 @@ "HasNonUniformScale": false } }, - { - "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", - "value": "{6D9F8E0A-466E-5774-8974-FC7F0872FD00}" - }, - { - "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", - "value": 1 - }, - { - "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", - "value": "physx/rubber.physicsmaterial" - }, - { - "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", - "value": 0.05999999865889549 - }, - { - "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", - "value": "panda_leftfinger" - }, { "op": "add", - "path": "/Entities/Entity_[755939860208]/Components/EditorColliderComponent", + "path": "/Entities/Entity_[807479467760]/Components/EditorDisabledCompositionComponent/DisabledComponents/EditorColliderComponent_2", "value": { "$type": "EditorColliderComponent", - "Id": 13507121597919349583, + "Id": 15762480907708745507, "ColliderConfiguration": { "CollisionLayer": { "Index": 0 @@ -2331,7 +2241,7 @@ "Exclusive": true, "Position": [ 0.0, - -0.004999999888241291, + 0.004999999888241291, 0.05000000074505806 ], "Rotation": [ @@ -2449,32 +2359,8 @@ }, { "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", - "value": "{6D9F8E0A-466E-5774-8974-FC7F0872FD00}" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", - "value": 1 - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", - "value": "physx/rubber.physicsmaterial" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", - "value": 0.05999999865889549 - }, - { - "op": "remove", - "path": "/Entities/Entity_[751644892912]/Components/EditorEntitySortComponent/Child Entity Order/3" - }, - { - "op": "replace", - "path": "/Entities/Entity_[751644892912]/Components/FingerGripperComponent/m_template/InitialPosition", - "value": 0.004999999888241291 + "path": "/Entities/Entity_[807479467760]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", + "value": "panda_leftfinger" } ] }, diff --git a/Project/Prefabs/PandaRobot.prefab b/Project/Prefabs/PandaRobot.prefab index 69d32fe..00b79c2 100644 --- a/Project/Prefabs/PandaRobot.prefab +++ b/Project/Prefabs/PandaRobot.prefab @@ -57,6 +57,110 @@ } }, "Entities": { + "Entity_[442577414367]": { + "Id": "Entity_[442577414367]", + "Name": "VacuumGripperCollider", + "Components": { + "EditorArticulationLinkComponent": { + "$type": "EditorArticulationLinkComponent", + "Id": 3531814366370572486, + "ArticulationConfiguration": { + "entityId": "" + } + }, + "EditorColliderComponent": { + "$type": "EditorColliderComponent", + "Id": 12351851051671595279, + "ColliderConfiguration": { + "Trigger": true, + "Position": [ + 0.0017166784964501858, + 0.002191312611103058, + 0.09619167447090149 + ], + "MaterialSlots": { + "Slots": [ + { + "Name": "Entire object" + } + ] + } + }, + "ShapeConfiguration": { + "ShapeType": 1, + "Box": { + "Configuration": [ + 0.037391602993011475, + 0.06567616015672684, + 0.07587969303131104 + ] + } + } + }, + "EditorDisabledCompositionComponent": { + "$type": "EditorDisabledCompositionComponent", + "Id": 90872317953177292 + }, + "EditorEntityIconComponent": { + "$type": "EditorEntityIconComponent", + "Id": 5018623141682882007 + }, + "EditorEntitySortComponent": { + "$type": "EditorEntitySortComponent", + "Id": 1618039179704572555 + }, + "EditorInspectorComponent": { + "$type": "EditorInspectorComponent", + "Id": 12001657184987035074 + }, + "EditorLockComponent": { + "$type": "EditorLockComponent", + "Id": 17643161545594275496 + }, + "EditorOnlyEntityComponent": { + "$type": "EditorOnlyEntityComponent", + "Id": 11503912829485453596 + }, + "EditorPendingCompositionComponent": { + "$type": "EditorPendingCompositionComponent", + "Id": 12229601031183600214 + }, + "EditorVisibilityComponent": { + "$type": "EditorVisibilityComponent", + "Id": 6855335127572019342 + }, + "GripperActionServerComponent": { + "$type": "GenericComponentWrapper", + "Id": 17486607982153163131, + "m_template": { + "$type": "GripperActionServerComponent", + "ActionServerName": "panda_hand_controller/vacuum_gripper_cmd" + } + }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 15458865587954632038, + "ROS2FrameConfiguration": { + "Frame Name": "", + "Publish Transform": false + } + }, + "TransformComponent": { + "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", + "Id": 6038403422994208093, + "Parent Entity": "Entity_[751644892912]" + }, + "VacuumGripperComponent": { + "$type": "GenericComponentWrapper", + "Id": 10195472909881418480, + "m_template": { + "$type": "VacuumGripperComponent", + "EffectorCollider": "Entity_[442577414367]", + "EffectorArticulation": "Entity_[442577414367]" + } + } + } + }, "Entity_[721580121840]": { "Id": "Entity_[721580121840]", "Name": "panda_link5", @@ -938,7 +1042,8 @@ "Entity_[811774435056]", "Entity_[755939860208]", "Entity_[807479467760]", - "Entity_[768824762096]" + "Entity_[768824762096]", + "Entity_[442577414367]" ] }, "EditorInspectorComponent": { @@ -1005,7 +1110,7 @@ }, "GripperActionServerComponent": { "$type": "GenericComponentWrapper", - "Id": 17571685017909850211, + "Id": 18293067359305499582, "m_template": { "$type": "GripperActionServerComponent", "ActionServerName": "panda_hand_controller/gripper_cmd" @@ -1061,7 +1166,50 @@ }, "EditorDisabledCompositionComponent": { "$type": "EditorDisabledCompositionComponent", - "Id": 7186072487014280539 + "Id": 7186072487014280539, + "DisabledComponents": { + "EditorMeshColliderComponent": { + "$type": "EditorMeshColliderComponent", + "Id": 3514899211479848914, + "ColliderConfiguration": { + "Rotation": [ + -0.0, + 0.0, + 1.0, + -1.0341155557737347e-13 + ], + "MaterialSlots": { + "Slots": [ + { + "Name": "DefaultMaterial" + } + ] + } + }, + "ShapeConfiguration": { + "PhysicsAsset": { + "Asset": { + "assetId": { + "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", + "subId": 3263612836 + }, + "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" + }, + "Configuration": { + "PhysicsAsset": { + "assetId": { + "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", + "subId": 3263612836 + }, + "loadBehavior": "QueueLoad", + "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" + }, + "UseMaterialsFromAsset": false + } + } + } + } + } }, "EditorEntityIconComponent": { "$type": "EditorEntityIconComponent", @@ -1082,47 +1230,6 @@ "$type": "EditorLockComponent", "Id": 13150366325223744978 }, - "EditorMeshColliderComponent": { - "$type": "EditorMeshColliderComponent", - "Id": 3514899211479848914, - "ColliderConfiguration": { - "Rotation": [ - -0.0, - 0.0, - 1.0, - -1.0341155557737347e-13 - ], - "MaterialSlots": { - "Slots": [ - { - "Name": "DefaultMaterial" - } - ] - } - }, - "ShapeConfiguration": { - "PhysicsAsset": { - "Asset": { - "assetId": { - "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", - "subId": 3263612836 - }, - "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" - }, - "Configuration": { - "PhysicsAsset": { - "assetId": { - "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", - "subId": 3263612836 - }, - "loadBehavior": "QueueLoad", - "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" - }, - "UseMaterialsFromAsset": false - } - } - } - }, "EditorOnlyEntityComponent": { "$type": "EditorOnlyEntityComponent", "Id": 6797995203671506988 @@ -2592,7 +2699,44 @@ }, "EditorDisabledCompositionComponent": { "$type": "EditorDisabledCompositionComponent", - "Id": 16198926508553308877 + "Id": 16198926508553308877, + "DisabledComponents": { + "EditorMeshColliderComponent": { + "$type": "EditorMeshColliderComponent", + "Id": 5443421632475784076, + "ColliderConfiguration": { + "MaterialSlots": { + "Slots": [ + { + "Name": "DefaultMaterial" + } + ] + } + }, + "ShapeConfiguration": { + "PhysicsAsset": { + "Asset": { + "assetId": { + "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", + "subId": 3263612836 + }, + "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" + }, + "Configuration": { + "PhysicsAsset": { + "assetId": { + "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", + "subId": 3263612836 + }, + "loadBehavior": "QueueLoad", + "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" + }, + "UseMaterialsFromAsset": false + } + } + } + } + } }, "EditorEntityIconComponent": { "$type": "EditorEntityIconComponent", @@ -2613,41 +2757,6 @@ "$type": "EditorLockComponent", "Id": 10109227038479545756 }, - "EditorMeshColliderComponent": { - "$type": "EditorMeshColliderComponent", - "Id": 5443421632475784076, - "ColliderConfiguration": { - "MaterialSlots": { - "Slots": [ - { - "Name": "DefaultMaterial" - } - ] - } - }, - "ShapeConfiguration": { - "PhysicsAsset": { - "Asset": { - "assetId": { - "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", - "subId": 3263612836 - }, - "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" - }, - "Configuration": { - "PhysicsAsset": { - "assetId": { - "guid": "{68A1F12D-6DF9-545E-B0E2-F8FCB87D86D0}", - "subId": 3263612836 - }, - "loadBehavior": "QueueLoad", - "assetHint": "assets/urdfimporter/1440394708_panda/finger.stl.pxmesh" - }, - "UseMaterialsFromAsset": false - } - } - } - }, "EditorOnlyEntityComponent": { "$type": "EditorOnlyEntityComponent", "Id": 18364140541148823701 diff --git a/Project/Prefabs/apple.prefab b/Project/Prefabs/apple.prefab index d3b041e..de088e0 100755 --- a/Project/Prefabs/apple.prefab +++ b/Project/Prefabs/apple.prefab @@ -136,8 +136,8 @@ "Id": 11883532161581818420, "Configuration": { "entityId": "", - "Linear damping": 20.0, - "Angular damping": 200.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Compute Mass": false, "Mass": 0.125, "Centre of mass offset": [ @@ -158,6 +158,13 @@ ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 9118820042985950900, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 17099443271266381888 diff --git a/Project/Prefabs/black_cube.prefab b/Project/Prefabs/black_cube.prefab index 8b73b82..972b0d0 100755 --- a/Project/Prefabs/black_cube.prefab +++ b/Project/Prefabs/black_cube.prefab @@ -137,22 +137,17 @@ "Id": 13561659260120629871, "Configuration": { "entityId": "", - "Linear damping": 10.0, - "Angular damping": 10.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Sleep threshold": 0.0, "Compute Mass": false, "Mass": 0.25, - "Centre of mass offset": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "Inertia tensor": [ - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, @@ -160,6 +155,13 @@ ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 761707688382702289, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 4403913835617255944 diff --git a/Project/Prefabs/blue_cube.prefab b/Project/Prefabs/blue_cube.prefab index 28198ea..d05643a 100755 --- a/Project/Prefabs/blue_cube.prefab +++ b/Project/Prefabs/blue_cube.prefab @@ -137,22 +137,17 @@ "Id": 13561659260120629871, "Configuration": { "entityId": "", - "Linear damping": 10.0, - "Angular damping": 10.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Sleep threshold": 0.0, "Compute Mass": false, "Mass": 0.25, - "Centre of mass offset": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "Inertia tensor": [ - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, @@ -160,6 +155,13 @@ ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 6285042727213339555, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 4403913835617255944 diff --git a/Project/Prefabs/carrot.prefab b/Project/Prefabs/carrot.prefab index 85ece79..0b3e841 100755 --- a/Project/Prefabs/carrot.prefab +++ b/Project/Prefabs/carrot.prefab @@ -353,8 +353,8 @@ "Id": 14790875665153150865, "Configuration": { "entityId": "", - "Linear damping": 20.0, - "Angular damping": 200.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Compute Mass": false, "Mass": 0.15000000596046448, "Centre of mass offset": [ @@ -375,6 +375,13 @@ ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 16397323468637220085, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 11409618612434829098 diff --git a/Project/Prefabs/corn.prefab b/Project/Prefabs/corn.prefab index 4cebcee..2d9f82b 100755 --- a/Project/Prefabs/corn.prefab +++ b/Project/Prefabs/corn.prefab @@ -255,28 +255,35 @@ "Id": 4476142177010692222, "Configuration": { "entityId": "", - "Linear damping": 20.0, - "Angular damping": 200.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Compute Mass": false, "Mass": 0.20000000298023224, "Centre of mass offset": [ - -0.0002216073771705851, - -0.0007254894007928669, - -0.0007085984689183533 + -0.0003693456237670034, + -0.0012091490207239985, + -0.0011809974675998092 ], "Inertia tensor": [ - 0.000046410106733674183, + 0.00012891695951111615, 0.0, 0.0, 0.0, - 0.000047158508095890284, + 0.0001309958752244711, 0.0, 0.0, 0.0, - 0.00001196084213006543 + 0.00003322456905152649 ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 11935729909519788024, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 12802023452715689730 diff --git a/Project/Prefabs/green_cube.prefab b/Project/Prefabs/green_cube.prefab index d48039f..47a2bea 100755 --- a/Project/Prefabs/green_cube.prefab +++ b/Project/Prefabs/green_cube.prefab @@ -137,22 +137,17 @@ "Id": 13561659260120629871, "Configuration": { "entityId": "", - "Linear damping": 10.0, - "Angular damping": 10.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Sleep threshold": 0.0, "Compute Mass": false, "Mass": 0.25, - "Centre of mass offset": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "Inertia tensor": [ - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, @@ -160,6 +155,13 @@ ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 9153336070195600087, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 4403913835617255944 diff --git a/Project/Prefabs/red_cube.prefab b/Project/Prefabs/red_cube.prefab index a79ae4b..9f8ab5e 100755 --- a/Project/Prefabs/red_cube.prefab +++ b/Project/Prefabs/red_cube.prefab @@ -137,22 +137,17 @@ "Id": 13561659260120629871, "Configuration": { "entityId": "", - "Linear damping": 10.0, - "Angular damping": 10.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Sleep threshold": 0.0, "Compute Mass": false, "Mass": 0.25, - "Centre of mass offset": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "Inertia tensor": [ - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, - 0.00010416666918899864, + 0.00010416667646495625, 0.0, 0.0, 0.0, @@ -160,6 +155,13 @@ ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 2837673088075890972, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 4403913835617255944 diff --git a/Project/Prefabs/tomato.prefab b/Project/Prefabs/tomato.prefab index b68ddf9..85bcd4a 100755 --- a/Project/Prefabs/tomato.prefab +++ b/Project/Prefabs/tomato.prefab @@ -136,28 +136,35 @@ "Id": 9224062442123972628, "Configuration": { "entityId": "", - "Linear damping": 20.0, - "Angular damping": 200.0, + "Linear damping": 1.0, + "Angular damping": 1.0, "Compute Mass": false, "Mass": 0.20000000298023224, "Centre of mass offset": [ - -0.00011431012535467744, - 0.00002530297206249088, - 0.001235659932717681 + -0.0001428876566933468, + 0.0000316287150781136, + 0.0015445752069354057 ], "Inertia tensor": [ - 0.000026813961085281335, + 0.00004189681931165978, 0.0, 0.0, 0.0, - 0.000029061740860925056, + 0.00004540897862170823, 0.0, 0.0, 0.0, - 0.00003083164119743742 + 0.00004817444641957991 ] } }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 4243647606867997133, + "Tags": [ + "Grippable" + ] + }, "EditorVisibilityComponent": { "$type": "EditorVisibilityComponent", "Id": 4910412103323004378 diff --git a/ros2_ws/src/robotic_manipulation/CMakeLists.txt b/ros2_ws/src/robotic_manipulation/CMakeLists.txt index 0561e2e..63c0c9d 100644 --- a/ros2_ws/src/robotic_manipulation/CMakeLists.txt +++ b/ros2_ws/src/robotic_manipulation/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(moveit_ros_planning_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(gazebo_msgs REQUIRED) find_package(vision_msgs REQUIRED) +find_package(control_msgs REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rai_interfaces REQUIRED) @@ -25,6 +26,7 @@ ament_target_dependencies( "rclcpp" "gazebo_msgs" "vision_msgs" + "control_msgs" "rosbag2_cpp" "rai_interfaces" ) diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h index 2bf709d..371121f 100644 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h +++ b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h @@ -17,6 +17,18 @@ #include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + class ArmController { public: @@ -56,6 +68,13 @@ class ArmController std::shared_ptr m_pandaArm; std::shared_ptr m_hand; + using GripperCommand = control_msgs::action::GripperCommand; + using GripperCommandClient = rclcpp_action::Client; + std::shared_ptr m_gripperClient; + std::string ActionName; + + bool SendGripperCommand(double position); + std::atomic_bool gripper = false; rclcpp::Node::SharedPtr m_node; diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 4d68921..fceeef2 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp @@ -17,12 +17,19 @@ #include #include +#include +#include +#include +#include +#include void ArmController::Initialize() { m_node = rclcpp::Node::make_shared("arm_controller"); m_node->set_parameter(rclcpp::Parameter("use_sim_time", true)); + ActionName = "panda_hand_controller/vacuum_gripper_cmd"; + WaitForClockMessage(); m_executor.add_node(m_node); @@ -31,6 +38,7 @@ void ArmController::Initialize() m_pandaArm = std::make_shared(m_node, "panda_arm"); m_hand = std::make_shared(m_node, "hand"); + m_gripperClient = rclcpp_action::create_client(m_node, ActionName); m_pandaArm->setMaxVelocityScalingFactor(1.0); m_pandaArm->setMaxAccelerationScalingFactor(1.0); @@ -90,19 +98,86 @@ bool ArmController::MoveThroughWaypoints(std::vector c void ArmController::Open() { gripper.store(true); + m_hand->setJointValueTarget("panda_finger_joint1", OpenGripperJointValue); while (m_hand->move() != moveit::core::MoveItErrorCode::SUCCESS) { RCLCPP_ERROR(m_node->get_logger(), "Failed to open hand"); } + + // Use action client to send open command. Small max_effort allows free opening. + if (!SendGripperCommand(1.0)) { + RCLCPP_ERROR(m_node->get_logger(), "Failed to open hand via gripper action"); + } } void ArmController::Close() { gripper.store(false); - m_hand->setJointValueTarget("panda_finger_joint1", ClosedGripperJointValue); + + m_hand->setJointValueTarget("panda_finger_joint1", 0.02); while (m_hand->move() != moveit::core::MoveItErrorCode::SUCCESS) { RCLCPP_ERROR(m_node->get_logger(), "Failed to close hand"); } + + // Use action client to send close command. Increase max_effort for gripping. + if (!SendGripperCommand(0.0)) { + RCLCPP_ERROR(m_node->get_logger(), "Failed to close hand via gripper action"); + } +} + +bool ArmController::SendGripperCommand(double position) +{ + auto logger = m_node->get_logger(); + if (!m_gripperClient) { + RCLCPP_ERROR(logger, "Gripper action client not initialized"); + return false; + } + + // Wait a short time for the action server to be available. + if (!m_gripperClient->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_ERROR(logger, "Gripper action server '%s' not available", ActionName.c_str()); + return false; + } + + GripperCommand::Goal goal_msg; + goal_msg.command.position = position; + + auto goal_handle_future = m_gripperClient->async_send_goal(goal_msg); + { + auto const timeout = std::chrono::milliseconds(10); + while (goal_handle_future.wait_for(timeout) != std::future_status::ready) { + // Give up CPU briefly — the other thread runs the executor and will + // make the future ready when the goal is accepted/rejected. + std::this_thread::yield(); + } + if (goal_handle_future.wait_for(std::chrono::seconds(0)) != + std::future_status::ready) { + RCLCPP_ERROR(logger, "Failed to send gripper goal"); + return false; + } + } + + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(logger, "Goal was rejected by gripper action server"); + return false; + } + + auto result_future = m_gripperClient->async_get_result(goal_handle); + { + auto const timeout = std::chrono::milliseconds(10); + while (result_future.wait_for(timeout) != std::future_status::ready) { + std::this_thread::yield(); + } + if (result_future.wait_for(std::chrono::seconds(0)) != + std::future_status::ready) { + RCLCPP_ERROR(logger, "Failed to get result from gripper action server"); + return false; + } + } + + auto result = result_future.get(); + return result.code == rclcpp_action::ResultCode::SUCCEEDED; } std::vector ArmController::GetEffectorPose() diff --git a/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp b/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp index 60ded72..f026744 100644 --- a/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp +++ b/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp @@ -42,9 +42,11 @@ void RaiManipulationInterfaceNode::Initialize(ArmController & arm) m_moveToService = m_node->create_service( "/manipulator_move_to", - [&]( + [&arm, this]( std::shared_ptr const request, std::shared_ptr response) { + auto logger = m_node->get_logger(); + RCLCPP_INFO(logger, "Received move request"); response->success = false; @@ -111,9 +113,10 @@ void RaiManipulationInterfaceNode::Initialize(ArmController & arm) m_resetService = m_node->create_service( "/reset_manipulator", - [&]( + [&arm, this]( [[maybe_unused]] std::shared_ptr const request, std::shared_ptr response) { + auto logger = m_node->get_logger(); RCLCPP_INFO(logger, "Received reset request"); response->success = arm.SetJointValues(m_startingPose); });