From aa65716a2ff02f0550f6171f7c703c59313790e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Tue, 28 Oct 2025 19:09:58 +0100 Subject: [PATCH 1/4] Fix lambda captures MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../src/rai_manipulation_interface_node.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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); }); From 7049ea4dc159a5dc2778009e34ca367090cf13bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 29 Oct 2025 21:29:13 +0100 Subject: [PATCH 2/4] Replace finger gripper with vacuum gripper MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../RoboticManipulationBenchmark.prefab | 130 +------- Project/Prefabs/PandaRobot.prefab | 293 ++++++++++++------ Project/Prefabs/apple.prefab | 7 + Project/Prefabs/black_cube.prefab | 16 +- Project/Prefabs/blue_cube.prefab | 16 +- Project/Prefabs/carrot.prefab | 7 + Project/Prefabs/corn.prefab | 19 +- Project/Prefabs/green_cube.prefab | 16 +- Project/Prefabs/red_cube.prefab | 16 +- Project/Prefabs/tomato.prefab | 19 +- .../src/robotic_manipulation/CMakeLists.txt | 2 + .../robotic_manipulation/arm_controller.h | 20 +- .../src/arm_controller.cpp | 76 ++++- 13 files changed, 372 insertions(+), 265 deletions(-) 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..896223c 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/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", @@ -925,7 +1029,16 @@ }, "EditorDisabledCompositionComponent": { "$type": "EditorDisabledCompositionComponent", - "Id": 6243996962595125816 + "Id": 6243996962595125816, + "DisabledComponents": { + "FingerGripperComponent": { + "$type": "GenericComponentWrapper", + "Id": 4674038609218834419, + "m_template": { + "$type": "FingerGripperComponent" + } + } + } }, "EditorEntityIconComponent": { "$type": "EditorEntityIconComponent", @@ -938,7 +1051,8 @@ "Entity_[811774435056]", "Entity_[755939860208]", "Entity_[807479467760]", - "Entity_[768824762096]" + "Entity_[768824762096]", + "Entity_[442577414367]" ] }, "EditorInspectorComponent": { @@ -996,21 +1110,6 @@ "$type": "EditorVisibilityComponent", "Id": 16131636600576526944 }, - "FingerGripperComponent": { - "$type": "GenericComponentWrapper", - "Id": 4674038609218834419, - "m_template": { - "$type": "FingerGripperComponent" - } - }, - "GripperActionServerComponent": { - "$type": "GenericComponentWrapper", - "Id": 17571685017909850211, - "m_template": { - "$type": "GripperActionServerComponent", - "ActionServerName": "panda_hand_controller/gripper_cmd" - } - }, "ROS2FrameEditorComponent": { "$type": "ROS2FrameEditorComponent", "Id": 15164989121167078780, @@ -1061,7 +1160,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 +1224,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 +2693,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 +2751,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..b2faa85 100755 --- a/Project/Prefabs/apple.prefab +++ b/Project/Prefabs/apple.prefab @@ -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..565f4f7 100755 --- a/Project/Prefabs/black_cube.prefab +++ b/Project/Prefabs/black_cube.prefab @@ -142,17 +142,12 @@ "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..02791c2 100755 --- a/Project/Prefabs/blue_cube.prefab +++ b/Project/Prefabs/blue_cube.prefab @@ -142,17 +142,12 @@ "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..b9045b6 100755 --- a/Project/Prefabs/carrot.prefab +++ b/Project/Prefabs/carrot.prefab @@ -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..f809cc1 100755 --- a/Project/Prefabs/corn.prefab +++ b/Project/Prefabs/corn.prefab @@ -260,23 +260,30 @@ "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..6d4eeff 100755 --- a/Project/Prefabs/green_cube.prefab +++ b/Project/Prefabs/green_cube.prefab @@ -142,17 +142,12 @@ "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..b15c56b 100755 --- a/Project/Prefabs/red_cube.prefab +++ b/Project/Prefabs/red_cube.prefab @@ -142,17 +142,12 @@ "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..7d1ba98 100755 --- a/Project/Prefabs/tomato.prefab +++ b/Project/Prefabs/tomato.prefab @@ -141,23 +141,30 @@ "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..74c47cd 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: @@ -54,7 +66,13 @@ class ArmController void WaitForClockMessage(); 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; diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 4d68921..04727ff 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/gripper_cmd"; + WaitForClockMessage(); m_executor.add_node(m_node); @@ -30,7 +37,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 +97,74 @@ 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); - 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() From 4062cc2288064870d9b73e113335eb7fd460ee1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Thu, 30 Oct 2025 13:58:12 +0100 Subject: [PATCH 3/4] Use fingers with vacuum gripper MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- Project/Prefabs/PandaRobot.prefab | 28 +++++++++++-------- .../robotic_manipulation/arm_controller.h | 1 + .../src/arm_controller.cpp | 15 +++++++++- 3 files changed, 32 insertions(+), 12 deletions(-) diff --git a/Project/Prefabs/PandaRobot.prefab b/Project/Prefabs/PandaRobot.prefab index 896223c..00b79c2 100644 --- a/Project/Prefabs/PandaRobot.prefab +++ b/Project/Prefabs/PandaRobot.prefab @@ -134,7 +134,7 @@ "Id": 17486607982153163131, "m_template": { "$type": "GripperActionServerComponent", - "ActionServerName": "panda_hand_controller/gripper_cmd" + "ActionServerName": "panda_hand_controller/vacuum_gripper_cmd" } }, "ROS2FrameEditorComponent": { @@ -1029,16 +1029,7 @@ }, "EditorDisabledCompositionComponent": { "$type": "EditorDisabledCompositionComponent", - "Id": 6243996962595125816, - "DisabledComponents": { - "FingerGripperComponent": { - "$type": "GenericComponentWrapper", - "Id": 4674038609218834419, - "m_template": { - "$type": "FingerGripperComponent" - } - } - } + "Id": 6243996962595125816 }, "EditorEntityIconComponent": { "$type": "EditorEntityIconComponent", @@ -1110,6 +1101,21 @@ "$type": "EditorVisibilityComponent", "Id": 16131636600576526944 }, + "FingerGripperComponent": { + "$type": "GenericComponentWrapper", + "Id": 4674038609218834419, + "m_template": { + "$type": "FingerGripperComponent" + } + }, + "GripperActionServerComponent": { + "$type": "GenericComponentWrapper", + "Id": 18293067359305499582, + "m_template": { + "$type": "GripperActionServerComponent", + "ActionServerName": "panda_hand_controller/gripper_cmd" + } + }, "ROS2FrameEditorComponent": { "$type": "ROS2FrameEditorComponent", "Id": 15164989121167078780, 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 74c47cd..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 @@ -66,6 +66,7 @@ class ArmController void WaitForClockMessage(); std::shared_ptr m_pandaArm; + std::shared_ptr m_hand; using GripperCommand = control_msgs::action::GripperCommand; using GripperCommandClient = rclcpp_action::Client; diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 04727ff..fceeef2 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp @@ -28,7 +28,7 @@ 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/gripper_cmd"; + ActionName = "panda_hand_controller/vacuum_gripper_cmd"; WaitForClockMessage(); @@ -37,6 +37,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); @@ -97,6 +98,12 @@ 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"); @@ -106,6 +113,12 @@ void ArmController::Open() void ArmController::Close() { gripper.store(false); + + 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"); From 29a67f7f53b582ac4fcc88e8b5cdd17bf9ad77e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Thu, 30 Oct 2025 15:18:59 +0100 Subject: [PATCH 4/4] Reduce damping in spawnable objects MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- Project/Prefabs/apple.prefab | 4 ++-- Project/Prefabs/black_cube.prefab | 4 ++-- Project/Prefabs/blue_cube.prefab | 4 ++-- Project/Prefabs/carrot.prefab | 4 ++-- Project/Prefabs/corn.prefab | 4 ++-- Project/Prefabs/green_cube.prefab | 4 ++-- Project/Prefabs/red_cube.prefab | 4 ++-- Project/Prefabs/tomato.prefab | 4 ++-- 8 files changed, 16 insertions(+), 16 deletions(-) diff --git a/Project/Prefabs/apple.prefab b/Project/Prefabs/apple.prefab index b2faa85..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": [ diff --git a/Project/Prefabs/black_cube.prefab b/Project/Prefabs/black_cube.prefab index 565f4f7..972b0d0 100755 --- a/Project/Prefabs/black_cube.prefab +++ b/Project/Prefabs/black_cube.prefab @@ -137,8 +137,8 @@ "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, diff --git a/Project/Prefabs/blue_cube.prefab b/Project/Prefabs/blue_cube.prefab index 02791c2..d05643a 100755 --- a/Project/Prefabs/blue_cube.prefab +++ b/Project/Prefabs/blue_cube.prefab @@ -137,8 +137,8 @@ "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, diff --git a/Project/Prefabs/carrot.prefab b/Project/Prefabs/carrot.prefab index b9045b6..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": [ diff --git a/Project/Prefabs/corn.prefab b/Project/Prefabs/corn.prefab index f809cc1..2d9f82b 100755 --- a/Project/Prefabs/corn.prefab +++ b/Project/Prefabs/corn.prefab @@ -255,8 +255,8 @@ "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": [ diff --git a/Project/Prefabs/green_cube.prefab b/Project/Prefabs/green_cube.prefab index 6d4eeff..47a2bea 100755 --- a/Project/Prefabs/green_cube.prefab +++ b/Project/Prefabs/green_cube.prefab @@ -137,8 +137,8 @@ "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, diff --git a/Project/Prefabs/red_cube.prefab b/Project/Prefabs/red_cube.prefab index b15c56b..9f8ab5e 100755 --- a/Project/Prefabs/red_cube.prefab +++ b/Project/Prefabs/red_cube.prefab @@ -137,8 +137,8 @@ "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, diff --git a/Project/Prefabs/tomato.prefab b/Project/Prefabs/tomato.prefab index 7d1ba98..85bcd4a 100755 --- a/Project/Prefabs/tomato.prefab +++ b/Project/Prefabs/tomato.prefab @@ -136,8 +136,8 @@ "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": [