Milestone of graspnet

dockerfiles/install_scripts/dev_packages.sh

@@ -9,4 +9,6 @@ source /home/$UNAME/.bashrc
 apt update
 
 apt install -y \
-    ros-$ROS_DISTRO-nmea-navsat-driver
+    ros-$ROS_DISTRO-nmea-navsat-driver \
+    ros-$ROS_DISTRO-moveit-ros-perception \
+    ros-$ROS_DISTRO-topic-tools

pyproject.toml

@@ -45,6 +45,9 @@ dependencies = [
     "transforms3d>=0.4.2",
     "waitress>=3.0.2",
     "xmltodict>=1.0.2",
+    "pandas>=2.2.0",
+    "trimesh>=4.8.2",
+    "nicegui>=3.0.4",
 ]
 
 [dependency-groups]
@@ -54,7 +57,12 @@ dev = [
     "ruff>=0.11.12",
     "ty>=0.0.1a7",
 ]
+graspnet = [
+    "graspnetpy",
+    ]
 
+[tool.uv.sources]
+graspnetpy = { git = "https://github.com/alliander-opensource/graspnet-baseline" }
 
 [tool.ruff.lint.isort]
 split-on-trailing-comma = false

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

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

ros2_ws/src/rcdt_franka_moveit_config/config/sensors_3d.yaml

@@ -0,0 +1,16 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+sensors:
+  - depth_image
+depth_image:
+  sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+  image_topic: ""
+  queue_size: 5
+  near_clipping_plane_distance: 0.3
+  far_clipping_plane_distance: 5.0
+  shadow_threshold: 0.2
+  padding_scale: 1.0
+  max_update_rate: 10.0
+  filtered_cloud_topic: ""

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 ########################

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),
         ]

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>

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()

ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar.license

@@ -0,0 +1,3 @@
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0

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),
+        ]
+    )

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>