277 make a ros wrapper (#285)

* Make a ros wrapper. Signed-off-by: Jelmer de Wolde <[email protected]> * Undo unnecessary changes. Signed-off-by: Jelmer de Wolde <[email protected]> * Introduce gui. Signed-off-by: Jelmer de Wolde <[email protected]> * Visualize gripper pose in Rviz. Signed-off-by: Jelmer de Wolde <[email protected]> * Correct gripper visualization with offset between wrist and tcp. Signed-off-by: Jelmer de Wolde <[email protected]> * Add animated visualization of plan. Signed-off-by: Jelmer de Wolde <[email protected]> * Clean up code. Signed-off-by: Jelmer de Wolde <[email protected]> --------- Signed-off-by: Jelmer de Wolde <[email protected]> Co-authored-by: Jelmer de Wolde <[email protected]> Signed-off-by: Jelmer de Wolde <[email protected]>
alliander-opensource · MaxWaterhout · Sep 23, 2025 · Oct 16, 2025 · Oct 21, 2025 · Oct 23, 2025
commit b925f9079df65d230942ac42c8986dda977517f8
diff --git a/pyproject.toml b/pyproject.toml
@@ -47,6 +47,7 @@ dependencies = [
     "xmltodict>=1.0.2",
     "pandas>=2.2.0",
     "trimesh>=4.8.2",
+    "nicegui>=3.0.4",
 ]
 
 [dependency-groups]
@@ -104,4 +105,4 @@ select = "DOC"
 style = "google"
 skip-checking-short-docstrings = false
 allow-init-docstring = true
-check-style-mismatch = true
+check-style-mismatch = true
diff --git a/ros2_ws/src/rcdt_franka_moveit_config/config/fr3.srdf b/ros2_ws/src/rcdt_franka_moveit_config/config/fr3.srdf
@@ -28,7 +28,7 @@ are defined
     group-->
     <!--SUBGROUPS:
     Groups can also be formed by referencing to already defined group names-->
-    <group name="fr3_arm">
+    <group name="arm">
         <joint name="fr3_joint1" />
         <joint name="fr3_joint2" />
         <joint name="fr3_joint3" />
@@ -37,10 +37,20 @@ are defined
         <joint name="fr3_joint6" />
         <joint name="fr3_joint7" />
     </group>
+    <group name="hand">
+        <joint name="fr3_joint8" />
+        <joint name="fr3_hand_joint" />
+        <joint name="fr3_hand_tcp_joint" />
+        <joint name="fr3_finger_joint1" />
+        <joint name="fr3_finger_joint2" />
+    </group>
+    <group name="tcp">
+        <link name="fr3_hand_tcp" />
+    </group>
     <!--GROUP_STATES:
     Purpose: Define a named state for a particular group, in terms of joint values. This is
     useful to define states like 'folded arms'-->
-    <group_state name="home" group="fr3_arm">
+    <group_state name="home" group="arm">
         <joint name="fr3_joint1" value="0" />
         <joint name="fr3_joint2" value="-0.78539816339" />
         <joint name="fr3_joint3" value="0" />
@@ -49,7 +59,7 @@ are defined
         <joint name="fr3_joint6" value="1.57079632679" />
         <joint name="fr3_joint7" value="0.78539816339" />
     </group_state>
-    <group_state name="drop" group="fr3_arm">
+    <group_state name="drop" group="arm">
         <joint name="fr3_joint1" value="-1.57079632679" />
         <joint name="fr3_joint2" value="-0.65" />
         <joint name="fr3_joint3" value="0" />
@@ -61,6 +71,7 @@ are defined
 
     <!--END_EFFECTOR:
     Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="fr3_link7" group="hand" parent_group="arm" />
     <!--DISABLE_COLLISIONS:
     By default it is assumed that any link of the robot could potentially come into
     collision with any other link in the robot. This tag disables collision checking between a

diff --git a/ros2_ws/src/rcdt_franka_moveit_config/config/kinematics.yaml b/ros2_ws/src/rcdt_franka_moveit_config/config/kinematics.yaml
@@ -2,7 +2,7 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-fr3_arm:
+arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001
diff --git a/ros2_ws/src/rcdt_franka_moveit_config/config/servo_params.yaml b/ros2_ws/src/rcdt_franka_moveit_config/config/servo_params.yaml
@@ -7,7 +7,7 @@
 thread_priority: 40
 publish_period: 0.001
 max_expected_latency: 0.1
-move_group_name: fr3_arm
+move_group_name: arm
 active_subgroup: ""
 
 ############################# INCOMING COMMAND SETTINGS ########################

diff --git a/ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py b/ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py
@@ -123,6 +123,7 @@ def generate_launch_description() -> LaunchDescription:
             world_arg.declaration,
             platforms_arg.declaration,
             positions_arg.declaration,
+            orientations_arg.declaration,
             bridge_topics_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]

diff --git a/ros2_ws/src/rcdt_gazebo/worlds/table_with_1_brick.sdf b/ros2_ws/src/rcdt_gazebo/worlds/table_with_1_brick.sdf
@@ -69,7 +69,7 @@ SPDX-License-Identifier: Apache-2.0
     </model>
 
     <model name='brick0'>
-      <pose>0.5 0 0 0 0 0</pose>
+      <pose>0.5 0 0.03 0 0 0</pose>
       <include>
         <uri>model://rcdt_gazebo/models/brick</uri>
       </include>

diff --git a/ros2_ws/src/rcdt_grasping/CMakeLists.txt b/ros2_ws/src/rcdt_grasping/CMakeLists.txt
@@ -0,0 +1,36 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.5)
+project(rcdt_grasping)
+
+# CMake dependencies:
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+# Other dependencies:
+find_package(rclpy REQUIRED)
+find_package(rcdt_utilities REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+# Python executables:
+install(
+  DIRECTORY src_py/
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Shared folders:
+install(
+  DIRECTORY launch 
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Default test:
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar b/ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar
diff --git a/ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar.license b/ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar.license
@@ -0,0 +1,3 @@
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0
diff --git a/ros2_ws/src/rcdt_grasping/launch/grasping.launch.py b/ros2_ws/src/rcdt_grasping/launch/grasping.launch.py
@@ -0,0 +1,42 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import Node
+from rcdt_utilities.register import Register
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """Setup the launch description for the node.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: A list of actions to be executed in the launch description.
+    """
+    graspnet_node = Node(
+        package="rcdt_grasping",
+        executable="generate_grasp.py",
+    )
+
+    return [
+        Register.on_log(
+            graspnet_node, "GraspNet model initialized and weights loaded", context
+        ),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate the launch description for the grasping node.
+
+    Returns:
+        LaunchDescription: The launch description containing the grasping node.
+    """
+    return LaunchDescription(
+        [
+            OpaqueFunction(function=launch_setup),
+        ]
+    )
diff --git a/ros2_ws/src/rcdt_grasping/package.xml b/ros2_ws/src/rcdt_grasping/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+
+<!--
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0
+-->
+
+<package format="3">
+  <name>rcdt_grasping</name>
+  <version>0.1.0</version>
+  <description>Package that contains functionality for grasping objects.</description>
+  <maintainer email="[email protected]">RCDT</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rcdt_utilities</depend>
+
+  <exec_depend>rclpy</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/ros2_ws/src/rcdt_grasping/rcdt_grasping/__init__.py b/ros2_ws/src/rcdt_grasping/rcdt_grasping/__init__.py
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@
		SPDX-FileCopyrightText: Alliander N. V.

		SPDX-License-Identifier: Apache-2.0