i made the custom simulation work und NUC

Install Documentation/Encountered_Errors_and_Solutions

+--- stderr: ur_description                                         
+Traceback (most recent call last):
+  File "/opt/ros/humble/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py", line 21, in <module>
+    from ament_package.templates import get_environment_hook_template_path
+ModuleNotFoundError: No module named 'ament_package'
+
+Solution:	sudo apt-get install python3-ament-package
+
+
+--- stderr: ur_description                                                       
+CMake Error at CMakeLists.txt:5 (find_package):
+  By not providing "Findament_cmake.cmake" in CMAKE_MODULE_PATH this project
+  has asked CMake to find a package configuration file provided by
+  "ament_cmake", but CMake did not find one.
+
+  Could not find a package configuration file provided by "ament_cmake" with
+  any of the following names:
+
+    ament_cmakeConfig.cmake
+    ament_cmake-config.cmake
+
+  Add the installation prefix of "ament_cmake" to CMAKE_PREFIX_PATH or set
+  "ament_cmake_DIR" to a directory containing one of the above files.  If
+  "ament_cmake" provides a separate development package or SDK, be sure it
+  has been installed.
+
+
+---
+Failed   <<< ur_description [0.33s, exited with code 1]
+Aborted  <<< ser_test [0.72s]                     
+
+Solution:	source /opt/ros/humble/setup.bash
+
+
+Package 'ur_simulation_gz' not found: "package 'ur_simulation_gz' not found, searching: ['/opt/ros/humble', '/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ur_moveit_config', '/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ur_description', '/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ser_test']"
+
+Solution:	git clone -b humble https://github.com/UniversalRobots/Universal_Robots_ROS2_GZ_Simulation.git
+
+[ruby $(which ign) gazebo-5] [Err] [SystemLoader.cc:94] Failed to load system plugin [libign_ros2_control-system.so] : couldn't find shared library.
+
+Solution:	sudo apt-get install ros-humble-ign-ros2-control
+
+
+[ERROR] [launch]: Caught exception in launch (see debug for traceback): No module named 'ur_moveit_config'
+
+Solution:	i reset the ur_moveit_config package
+
+
+ ros2 launch ur_simulation_gz ur_sim_moveit.launch.py 
+[INFO] [launch]: All log files can be found below /home/sochi/.ros/log/2025-02-26-18-05-38-068304-7k4-ros2ina-24094
+[INFO] [launch]: Default logging verbosity is set to INFO
+[ERROR] [launch]: Caught exception in launch (see debug for traceback): "package 'moveit_servo' not found, searching: ['/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ur_simulation_gz', '/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ur_moveit_config', '/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ur_description', '/home/sochi/robot-sensor/workspaces/COLCON_WS/install/ser_test', '/opt/ros/humble']"
+
+Solution:	 sudo apt install ros-humble-moveit-servo
+

Universal_Robots_ROS2_GZ_Simulation @ 26d21f5f

+Subproject commit 26d21f5f3904da057d906d911bcbad8e86376840

workspaces/COLCON_WS/src/moveit_servo/cmake/ament_cmake_export_dependencies-extras.cmake

+# generated from ament_cmake_export_dependencies/cmake/ament_cmake_export_dependencies-extras.cmake.in
+
+set(_exported_dependencies "control_msgs;control_toolbox;geometry_msgs;moveit_core;moveit_msgs;moveit_ros_planning;pluginlib;rclcpp;rclcpp_components;sensor_msgs;std_msgs;std_srvs;tf2_eigen;trajectory_msgs")
+
+find_package(ament_cmake_libraries QUIET REQUIRED)
+
+# find_package() all dependencies
+# and append their DEFINITIONS INCLUDE_DIRS, LIBRARIES, and LINK_FLAGS
+# variables to moveit_servo_DEFINITIONS, moveit_servo_INCLUDE_DIRS,
+# moveit_servo_LIBRARIES, and moveit_servo_LINK_FLAGS.
+# Additionally collect the direct dependency names in
+# moveit_servo_DEPENDENCIES as well as the recursive dependency names
+# in moveit_servo_RECURSIVE_DEPENDENCIES.
+if(NOT _exported_dependencies STREQUAL "")
+  find_package(ament_cmake_core QUIET REQUIRED)
+  set(moveit_servo_DEPENDENCIES ${_exported_dependencies})
+  set(moveit_servo_RECURSIVE_DEPENDENCIES ${_exported_dependencies})
+  set(_libraries)
+  foreach(_dep ${_exported_dependencies})
+    if(NOT ${_dep}_FOUND)
+      find_package("${_dep}" QUIET REQUIRED)
+    endif()
+    # if a package provides modern CMake interface targets use them
+    # exclusively assuming the classic CMake variables only exist for
+    # backward compatibility
+    set(use_modern_cmake FALSE)
+    if(NOT "${${_dep}_TARGETS}" STREQUAL "")
+      foreach(_target ${${_dep}_TARGETS})
+        # only use actual targets
+        # in case a package uses this variable for other content
+        if(TARGET "${_target}")
+          get_target_property(_include_dirs ${_target} INTERFACE_INCLUDE_DIRECTORIES)
+          if(_include_dirs)
+            list_append_unique(moveit_servo_INCLUDE_DIRS "${_include_dirs}")
+          endif()
+
+          get_target_property(_imported_configurations ${_target} IMPORTED_CONFIGURATIONS)
+          if(_imported_configurations)
+            string(TOUPPER "${_imported_configurations}" _imported_configurations)
+            if(DEBUG_CONFIGURATIONS)
+              string(TOUPPER "${DEBUG_CONFIGURATIONS}" _debug_configurations_uppercase)
+            else()
+              set(_debug_configurations_uppercase "DEBUG")
+            endif()
+            foreach(_imported_config ${_imported_configurations})
+              get_target_property(_imported_implib ${_target} IMPORTED_IMPLIB_${_imported_config})
+              if(_imported_implib)
+                set(_imported_implib_config "optimized")
+                if(${_imported_config} IN_LIST _debug_configurations_uppercase)
+                  set(_imported_implib_config "debug")
+                endif()
+                list(APPEND _libraries ${_imported_implib_config} ${_imported_implib})
+              else()
+                get_target_property(_imported_location ${_target} IMPORTED_LOCATION_${_imported_config})
+                if(_imported_location)
+                  list(APPEND _libraries "${_imported_location}")
+                endif()
+              endif()
+            endforeach()
+          endif()
+
+          get_target_property(_link_libraries ${_target} INTERFACE_LINK_LIBRARIES)
+          if(_link_libraries)
+            list(APPEND _libraries "${_link_libraries}")
+          endif()
+          set(use_modern_cmake TRUE)
+        endif()
+      endforeach()
+    endif()
+    if(NOT use_modern_cmake)
+      if(${_dep}_DEFINITIONS)
+        list_append_unique(moveit_servo_DEFINITIONS "${${_dep}_DEFINITIONS}")
+      endif()
+      if(${_dep}_INCLUDE_DIRS)
+        list_append_unique(moveit_servo_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}")
+      endif()
+      if(${_dep}_LIBRARIES)
+        list(APPEND _libraries "${${_dep}_LIBRARIES}")
+      endif()
+      if(${_dep}_LINK_FLAGS)
+        list_append_unique(moveit_servo_LINK_FLAGS "${${_dep}_LINK_FLAGS}")
+      endif()
+      if(${_dep}_RECURSIVE_DEPENDENCIES)
+        list_append_unique(moveit_servo_RECURSIVE_DEPENDENCIES "${${_dep}_RECURSIVE_DEPENDENCIES}")
+      endif()
+    endif()
+    if(_libraries)
+      ament_libraries_deduplicate(_libraries "${_libraries}")
+      list(APPEND moveit_servo_LIBRARIES "${_libraries}")
+    endif()
+  endforeach()
+endif()

workspaces/COLCON_WS/src/moveit_servo/cmake/ament_cmake_export_targets-extras.cmake

+# generated from ament_cmake_export_targets/cmake/ament_cmake_export_targets-extras.cmake.in
+
+set(_exported_targets "export_moveit_servo")
+
+# include all exported targets
+if(NOT _exported_targets STREQUAL "")
+  foreach(_target ${_exported_targets})
+    set(_export_file "${moveit_servo_DIR}/${_target}Export.cmake")
+    include("${_export_file}")
+
+    # extract the target names associated with the export
+    set(_regex "foreach\\((_cmake)?_expected_?[Tt]arget (IN ITEMS )?(.+)\\)")
+    file(
+      STRINGS "${_export_file}" _foreach_targets
+      REGEX "${_regex}")
+    list(LENGTH _foreach_targets _matches)
+    if(NOT _matches EQUAL 1)
+      message(FATAL_ERROR
+        "Failed to find exported target names in '${_export_file}'")
+    endif()
+    string(REGEX REPLACE "${_regex}" "\\3" _targets "${_foreach_targets}")
+    string(REPLACE " " ";" _targets "${_targets}")
+    list(LENGTH _targets _length)
+
+    list(APPEND moveit_servo_TARGETS ${_targets})
+  endforeach()
+endif()

workspaces/COLCON_WS/src/moveit_servo/cmake/export_moveit_servoExport-none.cmake

+#----------------------------------------------------------------
+# Generated CMake target import file for configuration "None".
+#----------------------------------------------------------------
+
+# Commands may need to know the format version.
+set(CMAKE_IMPORT_FILE_VERSION 1)
+
+# Import target "moveit_servo::moveit_servo_lib" for configuration "None"
+set_property(TARGET moveit_servo::moveit_servo_lib APPEND PROPERTY IMPORTED_CONFIGURATIONS NONE)
+set_target_properties(moveit_servo::moveit_servo_lib PROPERTIES
+  IMPORTED_LOCATION_NONE "${_IMPORT_PREFIX}/lib/libmoveit_servo_lib.so.2.5.8"
+  IMPORTED_SONAME_NONE "libmoveit_servo_lib.so.2.5.8"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS moveit_servo::moveit_servo_lib )
+list(APPEND _IMPORT_CHECK_FILES_FOR_moveit_servo::moveit_servo_lib "${_IMPORT_PREFIX}/lib/libmoveit_servo_lib.so.2.5.8" )
+
+# Import target "moveit_servo::moveit_servo_lib_parameters" for configuration "None"
+set_property(TARGET moveit_servo::moveit_servo_lib_parameters APPEND PROPERTY IMPORTED_CONFIGURATIONS NONE)
+set_target_properties(moveit_servo::moveit_servo_lib_parameters PROPERTIES
+  IMPORTED_LOCATION_NONE "${_IMPORT_PREFIX}/lib/libmoveit_servo_lib_parameters.so.2.5.8"
+  IMPORTED_SONAME_NONE "libmoveit_servo_lib_parameters.so.2.5.8"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS moveit_servo::moveit_servo_lib_parameters )
+list(APPEND _IMPORT_CHECK_FILES_FOR_moveit_servo::moveit_servo_lib_parameters "${_IMPORT_PREFIX}/lib/libmoveit_servo_lib_parameters.so.2.5.8" )
+
+# Import target "moveit_servo::pose_tracking" for configuration "None"
+set_property(TARGET moveit_servo::pose_tracking APPEND PROPERTY IMPORTED_CONFIGURATIONS NONE)
+set_target_properties(moveit_servo::pose_tracking PROPERTIES
+  IMPORTED_LOCATION_NONE "${_IMPORT_PREFIX}/lib/libpose_tracking.so"
+  IMPORTED_SONAME_NONE "libpose_tracking.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS moveit_servo::pose_tracking )
+list(APPEND _IMPORT_CHECK_FILES_FOR_moveit_servo::pose_tracking "${_IMPORT_PREFIX}/lib/libpose_tracking.so" )
+
+# Import target "moveit_servo::servo_node" for configuration "None"
+set_property(TARGET moveit_servo::servo_node APPEND PROPERTY IMPORTED_CONFIGURATIONS NONE)
+set_target_properties(moveit_servo::servo_node PROPERTIES
+  IMPORTED_LOCATION_NONE "${_IMPORT_PREFIX}/lib/libservo_node.so"
+  IMPORTED_SONAME_NONE "libservo_node.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS moveit_servo::servo_node )
+list(APPEND _IMPORT_CHECK_FILES_FOR_moveit_servo::servo_node "${_IMPORT_PREFIX}/lib/libservo_node.so" )
+
+# Import target "moveit_servo::servo_controller_input" for configuration "None"
+set_property(TARGET moveit_servo::servo_controller_input APPEND PROPERTY IMPORTED_CONFIGURATIONS NONE)
+set_target_properties(moveit_servo::servo_controller_input PROPERTIES
+  IMPORTED_LOCATION_NONE "${_IMPORT_PREFIX}/lib/libservo_controller_input.so"
+  IMPORTED_SONAME_NONE "libservo_controller_input.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS moveit_servo::servo_controller_input )
+list(APPEND _IMPORT_CHECK_FILES_FOR_moveit_servo::servo_controller_input "${_IMPORT_PREFIX}/lib/libservo_controller_input.so" )
+
+# Commands beyond this point should not need to know the version.
+set(CMAKE_IMPORT_FILE_VERSION)

workspaces/COLCON_WS/src/ur_moveit_config/cmake/ur_moveit_configConfig-version.cmake → workspaces/COLCON_WS/src/moveit_servo/cmake/moveit_servoConfig-version.cmake

 # generated from ament/cmake/core/templates/nameConfig-version.cmake.in
-set(PACKAGE_VERSION "2.5.1")
+set(PACKAGE_VERSION "2.5.8")
 
 set(PACKAGE_VERSION_EXACT False)
 set(PACKAGE_VERSION_COMPATIBLE False)

workspaces/COLCON_WS/src/ur_moveit_config/cmake/ur_moveit_configConfig.cmake → workspaces/COLCON_WS/src/moveit_servo/cmake/moveit_servoConfig.cmake

 # generated from ament/cmake/core/templates/nameConfig.cmake.in
 
 # prevent multiple inclusion
-if(_ur_moveit_config_CONFIG_INCLUDED)
+if(_moveit_servo_CONFIG_INCLUDED)
   # ensure to keep the found flag the same
-  if(NOT DEFINED ur_moveit_config_FOUND)
+  if(NOT DEFINED moveit_servo_FOUND)
     # explicitly set it to FALSE, otherwise CMake will set it to TRUE
-    set(ur_moveit_config_FOUND FALSE)
-  elseif(NOT ur_moveit_config_FOUND)
+    set(moveit_servo_FOUND FALSE)
+  elseif(NOT moveit_servo_FOUND)
     # use separate condition to avoid uninitialized variable warning
-    set(ur_moveit_config_FOUND FALSE)
+    set(moveit_servo_FOUND FALSE)
   endif()
   return()
 endif()
-set(_ur_moveit_config_CONFIG_INCLUDED TRUE)
+set(_moveit_servo_CONFIG_INCLUDED TRUE)
 
 # output package information
-if(NOT ur_moveit_config_FIND_QUIETLY)
-  message(STATUS "Found ur_moveit_config: 2.5.1 (${ur_moveit_config_DIR})")
+if(NOT moveit_servo_FIND_QUIETLY)
+  message(STATUS "Found moveit_servo: 2.5.8 (${moveit_servo_DIR})")
 endif()
 
 # warn when using a deprecated package
 if(NOT "" STREQUAL "")
-  set(_msg "Package 'ur_moveit_config' is deprecated")
+  set(_msg "Package 'moveit_servo' is deprecated")
   # append custom deprecation text if available
   if(NOT "" STREQUAL "TRUE")
     set(_msg "${_msg} ()")
   endif()
   # optionally quiet the deprecation message
-  if(NOT ${ur_moveit_config_DEPRECATED_QUIET})
+  if(NOT ${moveit_servo_DEPRECATED_QUIET})
     message(DEPRECATION "${_msg}")
   endif()
 endif()
 
 # flag package as ament-based to distinguish it after being find_package()-ed
-set(ur_moveit_config_FOUND_AMENT_PACKAGE TRUE)
+set(moveit_servo_FOUND_AMENT_PACKAGE TRUE)
 
 # include all config extra files
-set(_extras "")
+set(_extras "ament_cmake_export_targets-extras.cmake;ament_cmake_export_dependencies-extras.cmake")
 foreach(_extra ${_extras})
-  include("${ur_moveit_config_DIR}/${_extra}")
+  include("${moveit_servo_DIR}/${_extra}")
 endforeach()

workspaces/COLCON_WS/src/moveit_servo/config/demo_rviz_config.rviz

+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 628
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /servo_node/publish_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        panda_hand:
+          Value: true
+        panda_leftfinger:
+          Value: false
+        panda_link0:
+          Value: false
+        panda_link1:
+          Value: false
+        panda_link2:
+          Value: false
+        panda_link3:
+          Value: false
+        panda_link4:
+          Value: false
+        panda_link5:
+          Value: false
+        panda_link6:
+          Value: false
+        panda_link7:
+          Value: false
+        panda_link8:
+          Value: false
+        panda_rightfinger:
+          Value: false
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          panda_link0:
+            panda_link1:
+              panda_link2:
+                panda_link3:
+                  panda_link4:
+                    panda_link5:
+                      panda_link6:
+                        panda_link7:
+                          panda_link8:
+                            panda_hand:
+                              panda_leftfinger:
+                                {}
+                              panda_rightfinger:
+                                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.155569553375244
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.08608254045248032
+        Y: -0.20677587389945984
+        Z: 0.3424459993839264
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4603978991508484
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8753982782363892
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 857
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1586
+  X: 1179
+  Y: 393

workspaces/COLCON_WS/src/moveit_servo/config/demo_rviz_pose_tracking.rviz

+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 628
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        panda_hand:
+          Value: true
+        panda_leftfinger:
+          Value: false
+        panda_link0:
+          Value: false
+        panda_link1:
+          Value: false
+        panda_link2:
+          Value: false
+        panda_link3:
+          Value: false
+        panda_link4:
+          Value: false
+        panda_link5:
+          Value: false
+        panda_link6:
+          Value: false
+        panda_link7:
+          Value: false
+        panda_link8:
+          Value: false
+        panda_rightfinger:
+          Value: false
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          panda_link0:
+            panda_link1:
+              panda_link2:
+                panda_link3:
+                  panda_link4:
+                    panda_link5:
+                      panda_link6:
+                        panda_link7:
+                          panda_link8:
+                            panda_hand:
+                              panda_leftfinger:
+                                {}
+                              panda_rightfinger:
+                                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.155569553375244
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.08608254045248032
+        Y: -0.20677587389945984
+        Z: 0.3424459993839264
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4603978991508484
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8753982782363892
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 857
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1586
+  X: 1179
+  Y: 393

workspaces/COLCON_WS/src/moveit_servo/config/panda_simulated_config.yaml

+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+  # Scale parameters are only used if command_in_type=="unitless"
+  linear:  0.4  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
+  joint: 0.5
+
+# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
+## Properties of outgoing commands
+publish_period: 0.034  # 1/Nominal publish rate [seconds]
+low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
+command_out_type: trajectory_msgs/JointTrajectory
+
+# What to publish? Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: true
+publish_joint_accelerations: false
+
+## Plugins for smoothing outgoing commands
+smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+
+# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
+# which other nodes can use as a source for information about the planning environment.
+# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
+# then is_primary_planning_scene_monitor needs to be set to false.
+is_primary_planning_scene_monitor: true
+
+## MoveIt properties
+move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
+planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: panda_link8  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame:  panda_link0  # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
+
+## Topic names
+cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: /joint_states
+status_topic: ~/status # Publish status to this topic
+command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
+
+## Collision checking for the entire robot body
+check_collisions: true # Check collisions?
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

workspaces/COLCON_WS/src/moveit_servo/config/panda_simulated_config_pose_tracking.yaml

+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+  # Scale parameters are only used if command_in_type=="unitless"
+  linear:  0.6  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
+  joint: 0.01
+
+# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
+## Properties of outgoing commands
+publish_period: 0.034  # 1/Nominal publish rate [seconds]
+
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
+command_out_type: trajectory_msgs/JointTrajectory
+
+# What to publish? Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: true
+publish_joint_accelerations: false
+
+## Plugins for smoothing outgoing commands
+smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+
+## MoveIt properties
+move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
+planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: panda_hand  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame:  panda_hand  # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
+
+## Topic names
+cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: /joint_states
+status_topic: ~/status # Publish status to this topic
+command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
+
+## Collision checking for the entire robot body
+check_collisions: true # Check collisions?
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

workspaces/COLCON_WS/src/moveit_servo/config/pose_tracking_settings.yaml

+#################################
+# PID parameters for pose seeking
+#################################
+
+# Maximum value of error integral for all PID controllers
+windup_limit: 0.05
+
+# PID gains
+x_proportional_gain: 1.5
+y_proportional_gain: 1.5
+z_proportional_gain: 1.5
+x_integral_gain: 0.0
+y_integral_gain: 0.0
+z_integral_gain: 0.0
+x_derivative_gain: 0.0
+y_derivative_gain: 0.0
+z_derivative_gain: 0.0
+
+angular_proportional_gain: 0.5
+angular_integral_gain: 0.0
+angular_derivative_gain: 0.0

workspaces/COLCON_WS/src/moveit_servo/environment/library_path.dsv

+prepend-non-duplicate;LD_LIBRARY_PATH;lib

workspaces/COLCON_WS/src/moveit_servo/environment/library_path.sh

+# copied from ament_package/template/environment_hook/library_path.sh
+
+# detect if running on Darwin platform
+_UNAME=`uname -s`
+_IS_DARWIN=0
+if [ "$_UNAME" = "Darwin" ]; then
+  _IS_DARWIN=1
+fi
+unset _UNAME
+
+if [ $_IS_DARWIN -eq 0 ]; then
+  ament_prepend_unique_value LD_LIBRARY_PATH "$AMENT_CURRENT_PREFIX/lib"
+else
+  ament_prepend_unique_value DYLD_LIBRARY_PATH "$AMENT_CURRENT_PREFIX/lib"
+fi
+unset _IS_DARWIN

workspaces/COLCON_WS/src/moveit_servo/launch/pose_tracking_example.launch.py

+import os
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+from launch_param_builder import ParameterBuilder
+
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    # Get parameters for the Pose Tracking node
+    servo_params = {
+        "moveit_servo": ParameterBuilder("moveit_servo")
+        .yaml("config/pose_tracking_settings.yaml")
+        .yaml("config/panda_simulated_config_pose_tracking.yaml")
+        .to_dict()
+    }
+
+    # RViz
+    rviz_config_file = (
+        get_package_share_directory("moveit_servo")
+        + "/config/demo_rviz_pose_tracking.rviz"
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        # prefix=['xterm -e gdb -ex run --args'],
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[moveit_config.to_dict()],
+    )
+
+    # Publishes tf's for the robot
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="screen",
+        parameters=[moveit_config.robot_description],
+    )
+
+    # A node to publish world -> panda_link0 transform
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    pose_tracking_node = Node(
+        package="moveit_servo",
+        executable="servo_pose_tracking_demo",
+        # prefix=['xterm -e gdb -ex run --args'],
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+            servo_params,
+        ],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="screen",
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager-timeout",
+            "300",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    panda_arm_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    return LaunchDescription(
+        [
+            rviz_node,
+            static_tf,
+            pose_tracking_node,
+            ros2_control_node,
+            joint_state_broadcaster_spawner,
+            panda_arm_controller_spawner,
+            robot_state_publisher,
+        ]
+    )