From 39cffa461334a5d4f70f1d0d36d7f779d41f538a Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 13 Jun 2026 19:18:54 +0200 Subject: [PATCH 01/88] add ur3 to sausage universe --- CustomRobots/robot_arms/launch/ur3.launch.py | 417 +++++ CustomRobots/robot_arms/launch/ur5.launch.py | 3 + .../ur3/ros2srrc_ur3_gazebo/CMakeLists.txt | 44 + .../config/configurations.yaml | 20 + .../ur3/ros2srrc_ur3_gazebo/package.xml | 21 + .../ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro | 138 ++ .../urdf/ur3_robotiq_2f85.urdf.xacro | 167 ++ .../urdf/ur3_robotiq_hande.urdf.xacro | 149 ++ .../worlds/ros2srrc_ur3.world | 1611 +++++++++++++++++ .../worlds/ros2srrc_ur3_original.world | 59 + .../ur3/ros2srrc_ur3_moveit2/CMakeLists.txt | 43 + .../ur3/ros2srrc_ur3_moveit2/config/ur3.srdf | 73 + .../config/ur3_moveit2.rviz | 318 ++++ .../config/ur3robotiq_2f85.srdf | 210 +++ .../config/ur3robotiq_2f85_moveit2.rviz | 363 ++++ .../config/ur3robotiq_hande.srdf | 135 ++ .../config/ur3robotiq_hande_moveit2.rviz | 333 ++++ .../ur3/ros2srrc_ur3_moveit2/package.xml | 21 + .../urdf/robotiq_2f85_macro.urdf.xacro | 5 + .../urdf/robotiq_2f85_ros2control.xacro | 122 +- .../urdf/robotiq_2f85_transmission.xacro | 97 +- .../ros2srrc_execution/src/move.cpp | 31 +- .../ros2srrc_execution/src/robmove.cpp | 72 +- .../ros2srrc_execution/src/robpose.cpp | 10 +- .../ur3/config/controller.yaml | 88 +- .../ur3/config/moveit_controllers.yaml | 20 + .../ur3/config/ompl_planning.yaml | 67 + .../ur3/urdf/ur3_macro.urdf.xacro | 21 - .../ur3/urdf/ur3_ros2control.xacro | 2 +- database/universes.sql | 6 +- 30 files changed, 4437 insertions(+), 229 deletions(-) create mode 100644 CustomRobots/robot_arms/launch/ur3.launch.py create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml create mode 100644 Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml create mode 100644 Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py new file mode 100644 index 000000000..4b4c80a8e --- /dev/null +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -0,0 +1,417 @@ +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, + SetEnvironmentVariable, +) +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch import LaunchDescription +from launch_ros.actions import Node + +import os +import xacro +import yaml +from ament_index_python.packages import get_package_share_directory +from launch.actions import DeclareLaunchArgument, OpaqueFunction + + +def load_file(package_name, file_path): + pkg_path = get_package_share_directory(package_name) + with open(os.path.join(pkg_path, file_path), "r") as f: + return f.read() + + +def load_yaml(package_name, file_path): + pkg_path = get_package_share_directory(package_name) + with open(os.path.join(pkg_path, file_path), "r") as f: + return yaml.safe_load(f) + + +def launch_setup(context): + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + + print("X =", x.perform(context)) + print("Y =", y.perform(context)) + print("Z =", z.perform(context)) + + R = LaunchConfiguration("R") + P = LaunchConfiguration("P") + Y = LaunchConfiguration("Y") + gz_sensor = LaunchConfiguration("sensor") + + sensor = gz_sensor.perform(context) + + nodes = [] + + # ========================= + # ROBOT DESCRIPTION (URDF) + # ========================= + xacro_file = os.path.join( + get_package_share_directory("ros2srrc_ur3_gazebo"), + "urdf", + "ur3_robotiq_2f85.urdf.xacro", + ) + + robot_description_content = xacro.process_file( + xacro_file, + mappings={ + "bringup": "false", + "hmi": "false", + "robot_ip": "0.0.0.0", + "EE": "true", + "EE_name": "robotiq_2f85", + "camera": "true" if sensor == "camera" else "false", + "script_filename": "none", + "input_recipe_filename": "none", + "output_recipe_filename": "none", + }, + ).toxml() + + robot_description = {"robot_description": robot_description_content} + + # ========================= + # ENVIROMENT + # ========================= + + gz_ros2_control_path = "/home/ws/install/gz_ros2_control/lib" + gz_link_attacher_path = "/home/ws/install/gz_link_attacher/lib" + + gz_plugin_path = ( + gz_link_attacher_path + ":" + gz_ros2_control_path + ":" + "/opt/ros/humble/lib" + ) + + resource_path = ( + os.path.dirname(get_package_share_directory("ros2srrc_ur3_gazebo")) + + ":" + + os.path.dirname(get_package_share_directory("ros2srrc_robots")) + + ":" + + os.path.dirname(get_package_share_directory("ros2srrc_endeffectors")) + + ":" + + os.path.dirname(get_package_share_directory("robotiq_description")) + + ":" + + os.path.join( + get_package_share_directory("robotiq_description"), + "world", + "models", + ) + ) + + set_resource_path = SetEnvironmentVariable( + name="GZ_SIM_RESOURCE_PATH", value=resource_path + ) + + set_gz_plugin_path = SetEnvironmentVariable( + name="GZ_SIM_SYSTEM_PLUGIN_PATH", value=gz_plugin_path + ) + + existing_ld = os.environ.get("LD_LIBRARY_PATH", "") + + set_ld_library_path = SetEnvironmentVariable( + name="LD_LIBRARY_PATH", + value=gz_plugin_path + ":/usr/lib/x86_64-linux-gnu:" + existing_ld, + ) + + nodes.append(set_resource_path) + nodes.append(set_gz_plugin_path) + nodes.append(set_ld_library_path) + + # ========================= + # SRDF (SEMANTIC) + # ========================= + robot_description_semantic = { + "robot_description_semantic": load_file( + "ros2srrc_ur3_moveit2", + "config/ur3robotiq_2f85.srdf" + ) + } + + # ========================= + # KINEMATICS + # ========================= + kinematics_yaml = { + "robot_description_kinematics": load_yaml( + "ros2srrc_robots", + "ur3/config/kinematics.yaml" + ) + } + + + # ========================= + # Moveit controller + # ========================= + moveit_controllers = load_yaml( + "ros2srrc_robots", + "ur3/config/moveit_controllers.yaml" + ) + + moveit_controllers = moveit_controllers["/**"]["ros__parameters"] + + ompl_planning = load_yaml( + "ros2srrc_robots", + "ur3/config/ompl_planning.yaml" + ) + + ompl_planning = ompl_planning["/**"]["ros__parameters"] + + joint_limits_yaml = load_yaml( + "ros2srrc_robots", + "ur3/config/joint_limits.yaml" + ) + + pilz_cartesian_limits = load_yaml( + "ros2srrc_robots", + "ur3/config/pilz_cartesian_limits.yaml" + ) + + combined_planning = { + "robot_description_planning": + {**joint_limits_yaml, **pilz_cartesian_limits} + } + + planning_pipelines_config = { + "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], + "default_planning_pipeline": "pilz_industrial_motion_planner", + + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + }, + + "pilz_industrial_motion_planner": { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", + "request_adapters": "", + "start_state_max_bounds_error": 0.1, + "default_planner_config": "PTP", + }, + } + + move_group_capabilities = { + "capabilities": + "pilz_industrial_motion_planner/MoveGroupSequenceAction " + "pilz_industrial_motion_planner/MoveGroupSequenceService" + } + + # ========================= + # NODES + # ========================= + + move = Node( + package="ros2srrc_execution", + executable="move", + name="move_action_server", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + moveit_controllers, + ompl_planning, + { + "moveit_controller_manager": + "moveit_simple_controller_manager/MoveItSimpleControllerManager" + }, + combined_planning, + {"use_sim_time": True}, + {"ROB_PARAM": "ur3"}, + {"EE_PARAM": "robotiq_2f85"}, + {"MOVE_GROUP": "ur3_arm"}, + ], + ) + + robmove = Node( + package="ros2srrc_execution", + executable="robmove", + name="Robmove", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + moveit_controllers, + ompl_planning, + { + "moveit_controller_manager": + "moveit_simple_controller_manager/MoveItSimpleControllerManager" + }, + {"use_sim_time": True}, + {"ROB_PARAM": "ur3"}, + {"MOVE_GROUP": "ur3_arm"}, + ], + ) + + robpose = Node( + package="ros2srrc_execution", + executable="robpose", + name="robpose", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning, + {"use_sim_time": True}, + {"ROB_PARAM": "ur3"}, + {"MOVE_GROUP": "ur3_arm"}, + ], + ) + + nodes.append(move) + nodes.append(robmove) + nodes.append(robpose) + + # ========================= + # CORE NODES + # ========================= + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[robot_description, {"use_sim_time": True}], + output="screen", + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=[x, y, z, R, P, Y, "world", "base_link"], + ) + + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", + "robot_description", + "-name", + "ur3", + "-x", + x, + "-y", + y, + "-z", + z, + "-R", + R, + "-P", + P, + "-Y", + Y, + ], + output="screen", + ) + + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock]"], + parameters=[{"use_sim_time": True}], + ) + + sausage_spawner = Node( + package="custom_robots", + executable="spawn_sausage.py", + name="box_spawner", + output="screen", + ) + + nodes.append(robot_state_publisher) + nodes.append(static_tf) + nodes.append(spawn_robot) + nodes.append(clock_bridge) + #nodes.append(sausage_spawner) + + if sensor == "camera": + camera_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "/hand_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + "/base_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + ], + output="screen", + ) + + gz_ros2_image_bridge = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/hand_camera/image"], + output="screen", + ) + + gz_ros2_base_image_bridge = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/base_camera/image"], + output="screen", + ) + + nodes.append(camera_bridge) + nodes.append(gz_ros2_image_bridge) + nodes.append(gz_ros2_base_image_bridge) + + # ========================= + # CONTROLLERS + # ========================= + + joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + joint_trajectory_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller"], + ) + + gripper_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["gripper_controller"], + ) + + nodes.append(joint_state_broadcaster) + nodes.append(joint_trajectory_controller) + nodes.append(gripper_controller) + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + planning_pipelines_config, + move_group_capabilities, + moveit_controllers, + combined_planning, + {"use_sim_time": True}, + ], + ) + + nodes.append(move_group) + + # ========================= + # LAUNCH + # ========================= + + return nodes + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument("x", default_value="0.5"), + DeclareLaunchArgument("y", default_value="0"), + DeclareLaunchArgument("z", default_value="0.9"), + DeclareLaunchArgument("R", default_value="0"), + DeclareLaunchArgument("P", default_value="0"), + DeclareLaunchArgument("Y", default_value="0"), + DeclareLaunchArgument("sensor", default_value="none"), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/CustomRobots/robot_arms/launch/ur5.launch.py b/CustomRobots/robot_arms/launch/ur5.launch.py index 6668c5434..1b7c76e92 100644 --- a/CustomRobots/robot_arms/launch/ur5.launch.py +++ b/CustomRobots/robot_arms/launch/ur5.launch.py @@ -154,6 +154,7 @@ def launch_setup(context): {"use_sim_time": True}, {"ROB_PARAM": "ur5"}, {"EE_PARAM": "robotiq_2f85"}, + {"MOVE_GROUP": "ur5_manipulator"}, ], ) @@ -173,6 +174,7 @@ def launch_setup(context): }, {"use_sim_time": True}, {"ROB_PARAM": "ur5"}, + {"MOVE_GROUP": "ur5_manipulator"}, ], ) @@ -188,6 +190,7 @@ def launch_setup(context): ompl_planning, {"use_sim_time": True}, {"ROB_PARAM": "ur5"}, + {"MOVE_GROUP": "ur5_manipulator"}, ], ) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt new file mode 100644 index 000000000..b6bb475a3 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2srrc_ur3_gazebo) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY + config + urdf + worlds + DESTINATION + share/${PROJECT_NAME} +) + +ament_package() diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml new file mode 100644 index 000000000..f5ca050f7 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml @@ -0,0 +1,20 @@ +# ROS 2 Sim-to-Real Robot Control: UR3 Robot. +Configurations: + + - ID: "ur3_1" + Name: "UR3 on top of Robot Stand." + urdf: "ur3.urdf.xacro" + rob: "ur3" + ee: "none" + + - ID: "ur3_2" + Name: "UR3 + Robotiq 2f-85 gripper on top of Robot Stand." + urdf: "ur3_robotiq_2f85.urdf.xacro" + rob: "ur3" + ee: "robotiq_2f85" + + - ID: "ur3_3" + Name: "UR3 + Robotiq HandE gripper on top of Robot Stand." + urdf: "ur3_robotiq_hande.urdf.xacro" + rob: "ur3" + ee: "robotiq_hande" \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml new file mode 100644 index 000000000..c3462821e --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml @@ -0,0 +1,21 @@ + + + + ros2srrc_ur3_gazebo + 0.0.0 + IFRA-Cranfield (Cranfield University, UK). Package: ROS 2 SimRealRobotControl - UR3-Gazebo + + Mikel Bueno Viso + Mikel Bueno Viso + + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro new file mode 100644 index 000000000..3916bc6f3 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro @@ -0,0 +1,138 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find ros2srrc_robots)/ur3/config/controller.yaml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro new file mode 100644 index 000000000..2da941298 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find ros2srrc_robots)/ur3/config/controller.yaml + + + + $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro new file mode 100644 index 000000000..50d19205e --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find ros2srrc_robots)/ur3/config/controller.yaml + + + + $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world new file mode 100644 index 000000000..fa1e98d6d --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world @@ -0,0 +1,1611 @@ + + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.95 0 -0 0 + + + 0.45 0.35 0.5 + model://conveyor/meshes/conveyor.dae + + + + + 0 + 10 + 0 0 0.95 0 -0 0 + + + 2.4 0.8 0.1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + 0.335211 0.377742 0 0 -0 0 + + + 1 + + + 0 0 0.001 0 -0 0 + + + 1 1 1 + model://ground_logo_big/meshes/ground_logo.dae + + + + 0 + 0 + 0 + + -0.163565 0.817691 0 0 -0 0 + + + 1275 417000000 + 538 186819528 + 1594223523 209571631 + 534891 + + -0.6 0.45 0.755 0 -0 0 + 1 1 1 + + -0.6 0.45 0.755 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.6 0 0 0 -0 1.57 + 1 1 1 + + 0.6 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.5 0 0 0 -0 1.57 + 1 1 1 + + -0.5 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.6 0.15 0.755 0 -0 0 + 1 1 1 + + -0.6 0.15 0.755 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.6 0 0 0 -0 1.57 + 1 1 1 + + -0.6 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.6 -0.45 0.755 0 -0 0 + 1 1 1 + + -0.6 -0.45 0.755 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.6 -0.15 0.755 0 -0 0 + 1 1 1 + + -0.6 -0.15 0.755 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 3.82249 4.74978 2.99885 -0 0.401795 -2.20437 + orbit + perspective + + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.73 0 -0 1.57 + + + 1 1 1 + model://target_table/meshes/target_table.dae + + + + + 0 + 10 + 0 0 0.73 0 -0 1.57 + + + 0.8 1.6 0.06 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + -0.877342 -1.1194 0 0 -0 0 + + + 1 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.01 0 -0 0 + + + 0.5 0.5 0.5 + model://yellow_target/meshes/yellow_target.dae + + + + + 0 + 10 + 0.144 0 0 0 -0 0 + + + 0.012 0.3 0.24 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.144 0 0 -0 0 + + + 0.3 0.012 0.36 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.3 0.3 0.025 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.144 0 0 -0 0 + + + 0.3 0.012 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + -0.144 0 0 0 -0 0 + + + 0.012 0.3 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + -1.65226 -0.48866 0 0 -0 0 + + + 1 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.01 0 -0 0 + + + 0.5 0.5 0.5 + model://green_target/meshes/green_target.dae + + + + + 0 + 10 + 0.144 0 0 0 -0 0 + + + 0.012 0.3 0.24 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.144 0 0 -0 0 + + + 0.3 0.012 0.36 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.3 0.3 0.025 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.144 0 0 -0 0 + + + 0.3 0.012 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + -0.144 0 0 0 -0 0 + + + 0.012 0.3 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + -1.65226 -0.48866 0 0 -0 0 + + + 1 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.01 0 -0 0 + + + 0.5 0.5 0.5 + model://red_target/meshes/red_target.dae + + + + + 0 + 10 + 0.144 0 0 0 -0 0 + + + 0.012 0.3 0.24 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.144 0 0 -0 0 + + + 0.3 0.012 0.36 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.3 0.3 0.025 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.144 0 0 -0 0 + + + 0.3 0.012 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + -0.144 0 0 0 -0 0 + + + 0.012 0.3 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + -0.055993 0.310239 0 0 -0 0 + + + 1 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.01 0 -0 0 + + + 0.5 0.5 0.5 + model://blue_target/meshes/blue_target.dae + + + + + 0 + 10 + 0.144 0 0 0 -0 0 + + + 0.012 0.3 0.24 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.144 0 0 -0 0 + + + 0.3 0.012 0.36 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.3 0.3 0.025 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.144 0 0 -0 0 + + + 0.3 0.012 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + -0.144 0 0 0 -0 0 + + + 0.012 0.3 0.39 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + -1.18752 -1.7319 0 0 -0 0 + + + + + + + + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world new file mode 100644 index 000000000..00314e1d9 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world @@ -0,0 +1,59 @@ + + + + + + + + + + + 0 0 -9.8 + + + model://ground_plane + + + + model://sun + + + + 0 + + + + + + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt new file mode 100644 index 000000000..6e9f6ade0 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2srrc_ur3_moveit2) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# REQUIRED to -> Launch simulation: +install( + DIRECTORY + config + DESTINATION + share/${PROJECT_NAME} +) + +ament_package() diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf new file mode 100644 index 000000000..6aabb088e --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz new file mode 100644 index 000000000..bdaee0371 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz @@ -0,0 +1,318 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 348 + - 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 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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: + Value: true + - Acceleration_Scaling_Factor: 0.7 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + 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.009999999776482582 + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.7 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff010000022a000001880000017d00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf new file mode 100644 index 000000000..b7e7dd526 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf @@ -0,0 +1,210 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz new file mode 100644 index 000000000..1f96d17f7 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz @@ -0,0 +1,363 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 348 + - 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 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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: + Value: true + - Acceleration_Scaling_Factor: 0.7 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + 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.009999999776482582 + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.7 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff010000022a000001880000017d00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf new file mode 100644 index 000000000..a7f70bc79 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz new file mode 100644 index 000000000..aca1caf2d --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz @@ -0,0 +1,333 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 348 + - 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 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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: + Value: true + - Acceleration_Scaling_Factor: 0.7 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_hande_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_hande_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_hande_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + 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.009999999776482582 + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.7 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff010000022a000001880000017d00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml new file mode 100644 index 000000000..7f15f5caa --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml @@ -0,0 +1,21 @@ + + + + ros2srrc_ur3_moveit2 + 0.0.0 + IFRA-Cranfield (Cranfield University, UK). Package: ROS 2 SimRealRobotControl - UR3-MoveIt!2 + + Mikel Bueno Viso + Mikel Bueno Viso + + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro index 76a695acf..55aee203a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro @@ -168,6 +168,7 @@ + @@ -254,6 +255,7 @@ + @@ -284,6 +286,7 @@ + @@ -316,6 +319,7 @@ + @@ -348,6 +352,7 @@ + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro index 60d683304..f21a7a0ac 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro @@ -1,50 +1,15 @@ - - - + - + + - -0.05 + 0.0 0.80285 - - 0.5 - 0.5 - 0.0 @@ -52,86 +17,41 @@ + - - -0.05 - 0.80285 - - - 0.5 - 0.5 - - - 0.0 - + robotiq_85_left_knuckle_joint + 1.0 + - - -0.05 - 0.80285 - - - 0.5 - 0.5 - - - 0.0 - + robotiq_85_left_knuckle_joint + 1.0 + - - - -0.05 - 0.80285 - - - 0.5 - 0.5 - - - 0.0 - + robotiq_85_left_knuckle_joint + 1.0 + - - - -0.80285 - 0.05 - - - 0.5 - 0.5 - - - 0.0 - + robotiq_85_left_knuckle_joint + -1.0 + - - - -0.80285 - 0.05 - - - 0.5 - 0.5 - - - 0.0 - + robotiq_85_left_knuckle_joint + -1.0 + - - - - \ No newline at end of file + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro index 404658c65..20eca4250 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro @@ -1,128 +1,59 @@ - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + - - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - + 1 - - - - - transmission_interface/SimpleTransmission - hardware_interface/PositionJointInterface - - - 1 false + false + false + false + false + false + false + false + false - + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp index 4e72de49a..e09c50a21 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp @@ -76,6 +76,7 @@ std::string param_ROB = "none"; std::string param_EE = "none"; std::string param_ENV = "none"; +std::string param_MOVE_GROUP = "ur5_manipulator"; // Declaration of GLOBAL VARIABLES --> MoveIt!2 Interface: moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; @@ -119,6 +120,29 @@ class ros2_EEParam : public rclcpp::Node private: }; +class ros2_MoveGroupParam : public rclcpp::Node +{ +public: + ros2_MoveGroupParam() : Node("ros2_MoveGroupParam") + { + this->declare_parameter( + "MOVE_GROUP", + "ur5_manipulator" + ); + + param_MOVE_GROUP = + this->get_parameter("MOVE_GROUP") + .get_parameter_value() + .get(); + + RCLCPP_INFO( + this->get_logger(), + "MOVE_GROUP received -> %s", + param_MOVE_GROUP.c_str() + ); + } +}; + // ======================================================================================================================== // // ==================== FUNCTIONS ==================== // @@ -402,7 +426,7 @@ class ActionServer : public rclcpp::Node robot_trajectory::RobotTrajectory rt( move_group_interface_ROB.getRobotModel(), - "ur5_manipulator" + param_MOVE_GROUP ); moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); @@ -494,6 +518,9 @@ int main(int argc, char ** argv) auto node_PARAM_EE = std::make_shared(); rclcpp::spin_some(node_PARAM_EE); + auto node_PARAM_MOVE_GROUP = std::make_shared(); + rclcpp::spin_some(node_PARAM_MOVE_GROUP); + // ================= LOAD SPECS ================= if (param_ROB != "none"){ std::string pkgPATH_R = @@ -559,7 +586,7 @@ int main(int argc, char ** argv) // ROBOT if (param_ROB != "none"){ - std::string group_name = "ur5_manipulator"; + std::string group_name = param_MOVE_GROUP; move_group_interface_ROB = MoveGroupInterface(node2, group_name); diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index cfb982b46..7135e2791 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -52,6 +52,7 @@ moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; // Declaration of GLOBAL VARIABLE --> ROBOT PARAMETER: std::string param_ROB = "none"; +std::string param_MOVE_GROUP = "ur5_manipulator"; // Declaration of GLOBAL VARIABLE --> RES: std::string RES = "none"; @@ -91,6 +92,21 @@ class ActionServer : public rclcpp::Node explicit ActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("ros2srrc_RobMove", options){ this->declare_parameter("ROB_PARAM", "none"); + + this->declare_parameter( + "MOVE_GROUP", + "ur5_manipulator" + ); + + param_MOVE_GROUP = + this->get_parameter("MOVE_GROUP").as_string(); + + RCLCPP_INFO( + this->get_logger(), + "MOVE_GROUP received -> %s", + param_MOVE_GROUP.c_str() + ); + param_ROB = this->get_parameter("ROB_PARAM").as_string(); RCLCPP_INFO(this->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str()); @@ -135,13 +151,57 @@ class ActionServer : public rclcpp::Node void execute(const std::shared_ptr goal_handle) { + // ================= DEBUG ================= + RCLCPP_INFO( + get_logger(), + "Planning frame: %s", + move_group_interface_ROB.getPlanningFrame().c_str() + ); + + RCLCPP_INFO( + get_logger(), + "End effector link: %s", + move_group_interface_ROB.getEndEffectorLink().c_str() + ); + + auto state = move_group_interface_ROB.getCurrentState(); + + if (state) + { + const Eigen::Isometry3d& t = + state->getGlobalLinkTransform("tool0"); + + RCLCPP_INFO( + get_logger(), + "tool0 RobotState -> x=%.3f y=%.3f z=%.3f", + t.translation().x(), + t.translation().y(), + t.translation().z() + ); + } + + auto cp_tool0 = + move_group_interface_ROB.getCurrentPose("tool0"); + + RCLCPP_INFO( + get_logger(), + "tool0 CurrentPose -> x=%.3f y=%.3f z=%.3f", + cp_tool0.pose.position.x, + cp_tool0.pose.position.y, + cp_tool0.pose.position.z + ); - // 0. INFORMATION -> Current Robot Pose: auto CP_INFO = move_group_interface_ROB.getCurrentPose(); - RCLCPP_INFO(get_logger(), "INFORMATION -> Current Robot Pose:"); - RCLCPP_INFO(get_logger(), "POSITION -> (x: %.3f, y: %.3f, z: %.3f)", CP_INFO.pose.position.x, CP_INFO.pose.position.y, CP_INFO.pose.position.z); - RCLCPP_INFO(get_logger(), "ORIENTATION -> (qx: %.3f, qy: %.3f, qz: %.3f, qw: %.3f)", CP_INFO.pose.orientation.x, CP_INFO.pose.orientation.y, CP_INFO.pose.orientation.z, CP_INFO.pose.orientation.w); + RCLCPP_INFO( + get_logger(), + "Default CurrentPose -> x=%.3f y=%.3f z=%.3f", + CP_INFO.pose.position.x, + CP_INFO.pose.position.y, + CP_INFO.pose.position.z + ); + + // ================= FIN DEBUG ================= // 1. OBTAIN INPUT PARAMETERS: const auto GOAL = goal_handle->get_goal(); @@ -175,7 +235,7 @@ class ActionServer : public rclcpp::Node if (RES == "PLANNING: OK"){ robot_trajectory::RobotTrajectory rt( move_group_interface_ROB.getRobotModel(), - "ur5_manipulator" + param_MOVE_GROUP ); moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); @@ -271,7 +331,7 @@ int main(int argc, char **argv) using moveit::planning_interface::MoveGroupInterface; - std::string ROBname = "ur5_manipulator"; + std::string ROBname = param_MOVE_GROUP; move_group_interface_ROB = MoveGroupInterface(moveit_node, ROBname); diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 524a1e498..0d55554dc 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -52,6 +52,7 @@ moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; // Declaration of GLOBAL VARIABLE --> ROBOT PARAMETER: std::string param_ROB = "none"; +std::string param_MOVE_GROUP = "ur5_manipulator"; // Declaration of GLOBAL VARIABLE --> ROBOT POSE: ros2srrc_data::msg::Robpose POSE; @@ -109,13 +110,20 @@ int main(int argc, char **argv) // === PARAM === node->declare_parameter("ROB_PARAM", "none"); + node->declare_parameter( + "MOVE_GROUP", + "ur5_manipulator" + ); + param_ROB = node->get_parameter("ROB_PARAM").as_string(); + param_MOVE_GROUP = node->get_parameter("MOVE_GROUP").as_string(); + RCLCPP_INFO(node->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str()); // === MOVEIT === using moveit::planning_interface::MoveGroupInterface; - std::string ROBname = "ur5_manipulator"; + std::string ROBname = param_MOVE_GROUP; move_group_interface_ROB = MoveGroupInterface(node, ROBname); diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml index d5b1ac25c..26180e975 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml @@ -1,32 +1,96 @@ controller_manager: ros__parameters: - - update_rate: 250 #Hz + + update_rate: 100 joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +joint_state_broadcaster: + ros__parameters: + + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + - robotiq_85_left_knuckle_joint + + interfaces: + - position + - velocity + - effort + + joint_trajectory_controller: ros__parameters: + joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: - position + + state_interfaces: + - position - velocity + + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + + allow_partial_joints_goal: false + allow_nonzero_velocity_at_trajectory_end: true + allow_integration_in_goal_trajectories: true + + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + + +gripper_controller: + ros__parameters: + + joints: + - robotiq_85_left_knuckle_joint + + command_interfaces: + - position + state_interfaces: - position - velocity - state_publish_rate: 250.0 + + state_publish_rate: 100.0 action_monitor_rate: 20.0 + allow_partial_joints_goal: false + allow_integration_in_goal_trajectories: true + constraints: - stopped_velocity_tolerance: 0.0 - goal_time: 0.0 \ No newline at end of file + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + + robotiq_85_left_knuckle_joint: + trajectory: 0.5 + goal: 0.5 + + gains: + robotiq_85_left_knuckle_joint: + p: 100.0 + d: 1.0 + i: 0.01 + i_clamp: 1.0 \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml new file mode 100644 index 000000000..ef495fd5f --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + + moveit_simple_controller_manager: + + controller_names: + - joint_trajectory_controller + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml new file mode 100644 index 000000000..24eabd179 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml @@ -0,0 +1,67 @@ +/**: + ros__parameters: + + planner_configs: + + SBLkConfigDefault: + type: geometric::SBL + + ESTkConfigDefault: + type: geometric::EST + goal_bias: 0.05 + + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + border_fraction: 0.9 + min_valid_path_fraction: 0.5 + + BKPIECEkConfigDefault: + type: geometric::BKPIECE + border_fraction: 0.9 + failed_expansion_score_factor: 0.5 + min_valid_path_fraction: 0.5 + + KPIECEkConfigDefault: + type: geometric::KPIECE + goal_bias: 0.05 + border_fraction: 0.9 + failed_expansion_score_factor: 0.5 + min_valid_path_fraction: 0.5 + + RRTkConfigDefault: + type: geometric::RRT + goal_bias: 0.05 + + RRTConnectkConfigDefault: + type: geometric::RRTConnect + + RRTstarkConfigDefault: + type: geometric::RRTstar + goal_bias: 0.05 + delay_collision_checking: 1 + + TRRTkConfigDefault: + type: geometric::TRRT + goal_bias: 0.05 + + PRMkConfigDefault: + type: geometric::PRM + + PRMstarkConfigDefault: + type: geometric::PRMstar + + ur3_arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + + longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro index 9fafebecc..2219b142a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro @@ -99,9 +99,6 @@ - - - @@ -119,9 +116,6 @@ - - - @@ -139,9 +133,6 @@ - - - @@ -159,9 +150,6 @@ - - - @@ -179,9 +167,6 @@ - - - @@ -199,9 +184,6 @@ - - - @@ -219,9 +201,6 @@ - - - diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro index a317468c0..3900a0aff 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro @@ -144,7 +144,7 @@ - gazebo_ros2_control/GazeboSystem + gz_ros2_control/GazeboSimSystem diff --git a/database/universes.sql b/database/universes.sql index 238550680..0180ee518 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -198,7 +198,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 68 Rover 4wd Warehouse Low Noise 68 19 69 Rover 4wd Warehouse High Noise 68 20 70 Machine vision Harmonic world 70 24 -71 Conveyor Belt World 71 0 +71 Conveyor Belt World 71 26 \. -- @@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {-1.0,10.0,0.1,0.0,0.0,0.0} 68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 70 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {0.0,0.0,0.9,0.0,0.0,0.0} -71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} +71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {-0.5,0.0,0.95,0.0,0.0,0.0} \. -- @@ -299,6 +299,8 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config) FROM stdin 22 Turtlebot 3 Medium Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=med 23 Turtlebot 3 High Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=high 24 Ur5 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq sensor:=camera +25 Ur3 /home/ws/src/CustomRobots/ur3/launch/ur3.launch.py ur3_robotiq None +26 Ur3 Camera /home/ws/src/CustomRobots/ur3/launch/ur3.launch.py ur3_robotiq sensor:=camera \. -- From 74177570ad5c774437231153a7280aec99d6a608 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 13 Jun 2026 21:20:41 +0200 Subject: [PATCH 02/88] Fix database --- database/universes.sql | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/database/universes.sql b/database/universes.sql index 0180ee518..8f9997484 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -299,8 +299,8 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config) FROM stdin 22 Turtlebot 3 Medium Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=med 23 Turtlebot 3 High Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=high 24 Ur5 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq sensor:=camera -25 Ur3 /home/ws/src/CustomRobots/ur3/launch/ur3.launch.py ur3_robotiq None -26 Ur3 Camera /home/ws/src/CustomRobots/ur3/launch/ur3.launch.py ur3_robotiq sensor:=camera +25 Ur3 /home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py ur3_robotiq None +26 Ur3 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py ur3_robotiq sensor:=camera \. -- From 8dc5305095ba796980ef43f8600fe4764fb7bdd0 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 09:40:43 +0200 Subject: [PATCH 03/88] Fix database --- database/universes.sql | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/database/universes.sql b/database/universes.sql index 8f9997484..875cfe030 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {-1.0,10.0,0.1,0.0,0.0,0.0} 68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 70 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {0.0,0.0,0.9,0.0,0.0,0.0} -71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {-0.5,0.0,0.95,0.0,0.0,0.0} +71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {-0.5,0.0,0.0,0.0,0.0,0.0} \. -- From e69d00aeee4200ad70c65356e105f8f3b8fadd72 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 10:28:28 +0200 Subject: [PATCH 04/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 7135e2791..4f8c36782 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -223,6 +223,23 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z = GOAL->qz; TARGET_POSE.orientation.w = GOAL->qw; + RCLCPP_INFO( + get_logger(), + "TARGET -> x=%.3f y=%.3f z=%.3f", + TARGET_POSE.position.x, + TARGET_POSE.position.y, + TARGET_POSE.position.z + ); + + RCLCPP_INFO( + get_logger(), + "TARGET Q -> %.3f %.3f %.3f %.3f", + TARGET_POSE.orientation.x, + TARGET_POSE.orientation.y, + TARGET_POSE.orientation.z, + TARGET_POSE.orientation.w + ); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); move_group_interface_ROB.setStartStateToCurrentState(); From cd8a8f6fe6af872194da594f768a5029bbcb28e4 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 11:26:43 +0200 Subject: [PATCH 05/88] Try planification wothout camera --- database/universes.sql | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/database/universes.sql b/database/universes.sql index 875cfe030..8a18971a7 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -198,7 +198,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 68 Rover 4wd Warehouse Low Noise 68 19 69 Rover 4wd Warehouse High Noise 68 20 70 Machine vision Harmonic world 70 24 -71 Conveyor Belt World 71 26 +71 Conveyor Belt World 71 25 \. -- From 5d9930ef5c96a32648379a0ec61c4828cd956146 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 11:55:39 +0200 Subject: [PATCH 06/88] add if camera to ur3 xacro --- .../urdf/ur3_robotiq_2f85.urdf.xacro | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro index 2da941298..69935a0a8 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro @@ -58,20 +58,22 @@ - + - + - - - + + + + - + + + + + - - - - + From 97bd703976ee6a0ebcc5f65dcd39613aa4fd99ba Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 13:03:34 +0200 Subject: [PATCH 07/88] add debug to robpose.cpp --- .../ros2srrc_execution/src/robpose.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 0d55554dc..268138753 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -129,6 +129,18 @@ int main(int argc, char **argv) RCLCPP_INFO(node->get_logger(), "MoveGroupInterface created"); + RCLCPP_INFO( + node->get_logger(), + "End effector link: %s", + move_group_interface_ROB.getEndEffectorLink().c_str() + ); + + RCLCPP_INFO( + node->get_logger(), + "Planning frame: %s", + move_group_interface_ROB.getPlanningFrame().c_str() + ); + rclcpp::spin(node); rclcpp::shutdown(); From f19a764cb22630a9347a6f0a027d578359c325e6 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 13:26:46 +0200 Subject: [PATCH 08/88] add debug to robpose.cpp --- .../ros2srrc_execution/src/robpose.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 268138753..76f9b9a8a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -80,6 +80,12 @@ class RobPose_PUB : public rclcpp::Node auto CP_INFO = move_group_interface_ROB.getCurrentPose(); + RCLCPP_INFO( + this->get_logger(), + "CurrentPose frame_id = %s", + CP_INFO.header.frame_id.c_str() + ); + POSE.x = CP_INFO.pose.position.x; POSE.y = CP_INFO.pose.position.y; POSE.z = CP_INFO.pose.position.z; @@ -89,7 +95,6 @@ class RobPose_PUB : public rclcpp::Node POSE.qw = CP_INFO.pose.orientation.w; publisher_->publish(POSE); - } rclcpp::TimerBase::SharedPtr timer_; From f8a91f4ce0f366d4d3ce9346d94acab29d05cd9c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 17:37:31 +0200 Subject: [PATCH 09/88] Try to fix transformation --- .../ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro | 4 ++-- .../ros2srrc_execution/src/robpose.cpp | 7 +------ 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro index 69935a0a8..c2d166f75 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro @@ -143,14 +143,14 @@ - + - + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 76f9b9a8a..268138753 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -80,12 +80,6 @@ class RobPose_PUB : public rclcpp::Node auto CP_INFO = move_group_interface_ROB.getCurrentPose(); - RCLCPP_INFO( - this->get_logger(), - "CurrentPose frame_id = %s", - CP_INFO.header.frame_id.c_str() - ); - POSE.x = CP_INFO.pose.position.x; POSE.y = CP_INFO.pose.position.y; POSE.z = CP_INFO.pose.position.z; @@ -95,6 +89,7 @@ class RobPose_PUB : public rclcpp::Node POSE.qw = CP_INFO.pose.orientation.w; publisher_->publish(POSE); + } rclcpp::TimerBase::SharedPtr timer_; From a27c92e86c6a87e7ed8065726e74a1a938c29fcc Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 17:52:51 +0200 Subject: [PATCH 10/88] Add debug to robpose --- .../urdf/ur3_robotiq_2f85.urdf.xacro | 4 +-- .../ros2srrc_execution/src/robpose.cpp | 25 ++++++++++++++++++- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro index c2d166f75..69935a0a8 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro @@ -143,14 +143,14 @@ - + - + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 268138753..41db1d685 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -67,7 +67,7 @@ class RobPose_PUB : public rclcpp::Node : Node("ros2srrc_RobPosePUB"), count_(0) { publisher_ = this->create_publisher("Robpose", 10); - timer_ = this->create_wall_timer(50ms, std::bind(&RobPose_PUB::timer_callback, this)); + timer_ = this->create_wall_timer(2000ms, std::bind(&RobPose_PUB::timer_callback, this)); } private: @@ -80,6 +80,29 @@ class RobPose_PUB : public rclcpp::Node auto CP_INFO = move_group_interface_ROB.getCurrentPose(); + RCLCPP_INFO( + this->get_logger(), + "CurrentPose frame_id = %s", + CP_INFO.header.frame_id.c_str() + ); + + RCLCPP_INFO( + this->get_logger(), + "MoveIt Pose -> x=%.6f y=%.6f z=%.6f", + CP_INFO.pose.position.x, + CP_INFO.pose.position.y, + CP_INFO.pose.position.z + ); + + RCLCPP_INFO( + this->get_logger(), + "MoveIt Quat -> qx=%.6f qy=%.6f qz=%.6f qw=%.6f", + CP_INFO.pose.orientation.x, + CP_INFO.pose.orientation.y, + CP_INFO.pose.orientation.z, + CP_INFO.pose.orientation.w + ); + POSE.x = CP_INFO.pose.position.x; POSE.y = CP_INFO.pose.position.y; POSE.z = CP_INFO.pose.position.z; From a839cfcc314c63b02d34c50cc0fa30f40bae1e56 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 18:13:09 +0200 Subject: [PATCH 11/88] Add mor debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 46 ++++++++++++++++++- 1 file changed, 45 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 4f8c36782..392d7d8bd 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -183,14 +183,27 @@ class ActionServer : public rclcpp::Node auto cp_tool0 = move_group_interface_ROB.getCurrentPose("tool0"); + auto cp_ee = + move_group_interface_ROB.getCurrentPose( + move_group_interface_ROB.getEndEffectorLink() + ); + RCLCPP_INFO( get_logger(), - "tool0 CurrentPose -> x=%.3f y=%.3f z=%.3f", + "tool0 -> %.3f %.3f %.3f", cp_tool0.pose.position.x, cp_tool0.pose.position.y, cp_tool0.pose.position.z ); + RCLCPP_INFO( + get_logger(), + "EEF -> %.3f %.3f %.3f", + cp_ee.pose.position.x, + cp_ee.pose.position.y, + cp_ee.pose.position.z + ); + auto CP_INFO = move_group_interface_ROB.getCurrentPose(); RCLCPP_INFO( @@ -240,8 +253,39 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.w ); + RCLCPP_INFO( + get_logger(), + "Current pose before planning -> x=%.6f y=%.6f z=%.6f", + CURRENT_POSE.pose.position.x, + CURRENT_POSE.pose.position.y, + CURRENT_POSE.pose.position.z + ); + + RCLCPP_INFO( + get_logger(), + "Current pose frame -> %s", + CURRENT_POSE.header.frame_id.c_str() + ); + + RCLCPP_INFO( + get_logger(), + "End effector link = %s", + move_group_interface_ROB.getEndEffectorLink().c_str() + ); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); + std::vector pose_target; + + bool has_target = + move_group_interface_ROB.getPoseTarget(pose_target); + + RCLCPP_INFO( + get_logger(), + "Pose target stored = %s", + has_target ? "YES" : "NO" + ); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); From e18c148f29a1de40d61e6a9d7e50628800e3576c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 18:29:58 +0200 Subject: [PATCH 12/88] Add mor debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 392d7d8bd..6c1a5142f 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -275,15 +275,21 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.setPoseTarget(TARGET_POSE); - std::vector pose_target; + auto stored_target = + move_group_interface_ROB.getPoseTarget(); - bool has_target = - move_group_interface_ROB.getPoseTarget(pose_target); + RCLCPP_INFO( + get_logger(), + "Stored target -> x=%.6f y=%.6f z=%.6f", + stored_target.pose.position.x, + stored_target.pose.position.y, + stored_target.pose.position.z + ); RCLCPP_INFO( get_logger(), - "Pose target stored = %s", - has_target ? "YES" : "NO" + "Stored target frame -> %s", + stored_target.header.frame_id.c_str() ); move_group_interface_ROB.setStartStateToCurrentState(); From 37d3dd13214ebc66d22e682ec8483466436e76e7 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 18:51:54 +0200 Subject: [PATCH 13/88] Add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 6c1a5142f..b53964982 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -227,6 +227,25 @@ class ActionServer : public rclcpp::Node auto CURRENT_POSE = move_group_interface_ROB.getCurrentPose(); + auto CURRENT_POSE_TOOL0 = + move_group_interface_ROB.getCurrentPose("tool0"); + + RCLCPP_INFO( + get_logger(), + "CURRENT_POSE(default) -> %.6f %.6f %.6f", + CURRENT_POSE.pose.position.x, + CURRENT_POSE.pose.position.y, + CURRENT_POSE.pose.position.z + ); + + RCLCPP_INFO( + get_logger(), + "CURRENT_POSE(tool0) -> %.6f %.6f %.6f", + CURRENT_POSE_TOOL0.pose.position.x, + CURRENT_POSE_TOOL0.pose.position.y, + CURRENT_POSE_TOOL0.pose.position.z + ); + geometry_msgs::msg::Pose TARGET_POSE; TARGET_POSE.position.x = GOAL->x; TARGET_POSE.position.y = GOAL->y; From 0b0798045865421c2b547a2d5f98a6ae29ec55f0 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 19:13:25 +0200 Subject: [PATCH 14/88] Add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index b53964982..ade1b62c2 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -311,13 +311,30 @@ class ActionServer : public rclcpp::Node stored_target.header.frame_id.c_str() ); - move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed); + /* NUEVO DEBUG */ + move_group_interface_ROB.setPlanningTime(10.0); + + RCLCPP_INFO( + get_logger(), + "Planner ID = %s", + move_group_interface_ROB.getPlannerId().c_str() + ); + + /* PLANIFICAR */ MyPlan = plan_ROB(); + /* NUEVO DEBUG */ + RCLCPP_INFO( + get_logger(), + "Planning result = %s", + RES.c_str() + ); + if (RES == "PLANNING: OK"){ robot_trajectory::RobotTrajectory rt( move_group_interface_ROB.getRobotModel(), From bfbead70dbfc481ffd6936df4556a9bbda2c70a8 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 19:14:03 +0200 Subject: [PATCH 15/88] Change ur3 launcher --- CustomRobots/robot_arms/launch/ur3.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index 4b4c80a8e..3cb8cf57d 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -403,9 +403,9 @@ def launch_setup(context): def generate_launch_description(): declared_arguments = [ - DeclareLaunchArgument("x", default_value="0.5"), + DeclareLaunchArgument("x", default_value="0"), DeclareLaunchArgument("y", default_value="0"), - DeclareLaunchArgument("z", default_value="0.9"), + DeclareLaunchArgument("z", default_value="0"), DeclareLaunchArgument("R", default_value="0"), DeclareLaunchArgument("P", default_value="0"), DeclareLaunchArgument("Y", default_value="0"), From 97bb8b6f81f53b380698596e2cbaab1a09bc0b74 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 20:15:19 +0200 Subject: [PATCH 16/88] Add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 47 ++++++++++++++++--- 1 file changed, 40 insertions(+), 7 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index ade1b62c2..f8031d819 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -60,24 +60,57 @@ std::string RES = "none"; // =============================================================================== // // MoveIt!2 -> MoveGroupInterface/Plan function: -moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() { - +moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() +{ moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_interface_ROB.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - // Execute the plan + RCLCPP_INFO( + rclcpp::get_logger("PLAN_DEBUG"), + "Pose target frame = %s", + move_group_interface_ROB.getPoseTarget().header.frame_id.c_str() + ); + + auto cp = move_group_interface_ROB.getCurrentPose(); + + RCLCPP_INFO( + rclcpp::get_logger("PLAN_DEBUG"), + "Current pose = %.3f %.3f %.3f", + cp.pose.position.x, + cp.pose.position.y, + cp.pose.position.z + ); + + auto result = move_group_interface_ROB.plan(my_plan); + + RCLCPP_INFO( + rclcpp::get_logger("PLAN_DEBUG"), + "MoveIt plan result code = %d", + result.val + ); + + bool success = + (result == moveit::planning_interface::MoveItErrorCode::SUCCESS); + + if (!success) + { + RCLCPP_ERROR( + rclcpp::get_logger("PLAN_DEBUG"), + "Planning failed. Error code = %d", + result.val + ); + } + if (success) { RES = "PLANNING: OK"; - return(my_plan); } else { RES = "PLANNING: ERROR"; - return(my_plan); } -}; + return my_plan; +} // =============================================================================== // // ROS2 Action Server to move the ROBOT: From adc14c884b5047f186bee1dbab3d6c644f5f58cd Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 22:14:03 +0200 Subject: [PATCH 17/88] Add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index f8031d819..c2f68d6d0 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -325,6 +325,25 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.getEndEffectorLink().c_str() ); + auto state = move_group_interface_ROB.getCurrentState(); + + std::vector joints; + + state->copyJointGroupPositions( + state->getJointModelGroup(param_MOVE_GROUP), + joints + ); + + for (size_t i = 0; i < joints.size(); i++) + { + RCLCPP_INFO( + get_logger(), + "Joint %ld = %.6f", + i+1, + joints[i] + ); + } + move_group_interface_ROB.setPoseTarget(TARGET_POSE); auto stored_target = From ce657737abcd7e198a59530cc314cfc6dc8fa424 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 22:32:08 +0200 Subject: [PATCH 18/88] Fix robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index c2f68d6d0..89ee23251 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -325,12 +325,13 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.getEndEffectorLink().c_str() ); - auto state = move_group_interface_ROB.getCurrentState(); + auto current_state_debug = + move_group_interface_ROB.getCurrentState(); std::vector joints; - state->copyJointGroupPositions( - state->getJointModelGroup(param_MOVE_GROUP), + current_state_debug->copyJointGroupPositions( + current_state_debug->getJointModelGroup(param_MOVE_GROUP), joints ); @@ -339,7 +340,7 @@ class ActionServer : public rclcpp::Node RCLCPP_INFO( get_logger(), "Joint %ld = %.6f", - i+1, + i + 1, joints[i] ); } From 0b531d0c86ba0eca9f47484ce3033506aaf0e2ce Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 22:57:55 +0200 Subject: [PATCH 19/88] Fix robpose.cpp --- .../ros2srrc_execution/src/robpose.cpp | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 41db1d685..4eab29e43 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -148,7 +148,25 @@ int main(int argc, char **argv) using moveit::planning_interface::MoveGroupInterface; std::string ROBname = param_MOVE_GROUP; - move_group_interface_ROB = MoveGroupInterface(node, ROBname); + auto moveit_node = std::make_shared( + "moveit_helper_node_robpose", + rclcpp::NodeOptions() + .automatically_declare_parameters_from_overrides(true) + ); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(moveit_node); + + std::thread([&executor]() { + executor.spin(); + }).detach(); + + move_group_interface_ROB = + MoveGroupInterface(moveit_node, ROBname); + + move_group_interface_ROB.startStateMonitor(); + + rclcpp::sleep_for(std::chrono::seconds(2)); RCLCPP_INFO(node->get_logger(), "MoveGroupInterface created"); From d8cf5936746c8311c97ad12a2a3ee4e20c1b8abc Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 23:25:58 +0200 Subject: [PATCH 20/88] Add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 83 +++++++++++++++++-- 1 file changed, 77 insertions(+), 6 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 89ee23251..1efe97b22 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -93,11 +93,29 @@ moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() if (!success) { - RCLCPP_ERROR( - rclcpp::get_logger("PLAN_DEBUG"), - "Planning failed. Error code = %d", - result.val - ); + switch(result.val) + { + case -31: + RCLCPP_ERROR( + rclcpp::get_logger("PLAN_DEBUG"), + "NO IK SOLUTION (-31)" + ); + break; + + case -2: + RCLCPP_ERROR( + rclcpp::get_logger("PLAN_DEBUG"), + "INVALID MOTION PLAN (-2)" + ); + break; + + default: + RCLCPP_ERROR( + rclcpp::get_logger("PLAN_DEBUG"), + "ERROR CODE = %d", + result.val + ); + } } if (success) @@ -326,7 +344,16 @@ class ActionServer : public rclcpp::Node ); auto current_state_debug = - move_group_interface_ROB.getCurrentState(); + move_group_interface_ROB.getCurrentState(5.0); + + if (!current_state_debug) + { + RCLCPP_ERROR( + get_logger(), + "getCurrentState() returned nullptr" + ); + return; + } std::vector joints; @@ -345,6 +372,41 @@ class ActionServer : public rclcpp::Node ); } + bool ik_ok = + current_state_debug->setFromIK( + current_state_debug->getJointModelGroup(param_MOVE_GROUP), + TARGET_POSE, + "tool0", + 5, + 0.1 + ); + + RCLCPP_INFO( + get_logger(), + "Direct IK result = %s", + ik_ok ? "SUCCESS" : "FAILED" + ); + + if (ik_ok) + { + std::vector ik_joints; + + current_state_debug->copyJointGroupPositions( + current_state_debug->getJointModelGroup(param_MOVE_GROUP), + ik_joints + ); + + for (size_t i = 0; i < ik_joints.size(); i++) + { + RCLCPP_INFO( + get_logger(), + "IK Joint %ld = %.6f", + i + 1, + ik_joints[i] + ); + } + } + move_group_interface_ROB.setPoseTarget(TARGET_POSE); auto stored_target = @@ -487,9 +549,18 @@ int main(int argc, char **argv) using moveit::planning_interface::MoveGroupInterface; + RCLCPP_INFO( + logger, + "param_MOVE_GROUP = %s", + param_MOVE_GROUP.c_str() + ); + std::string ROBname = param_MOVE_GROUP; move_group_interface_ROB = MoveGroupInterface(moveit_node, ROBname); + + move_group_interface_ROB.startStateMonitor(); + rclcpp::sleep_for(std::chrono::seconds(2)); move_group_interface_ROB.setMaxVelocityScalingFactor(1.0); move_group_interface_ROB.setMaxAccelerationScalingFactor(1.0); From 313884955908dcbdaf852656afcd298646008bc8 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 23:37:36 +0200 Subject: [PATCH 21/88] Add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 75 +++++++++++++++++-- 1 file changed, 70 insertions(+), 5 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 1efe97b22..16097a598 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -355,10 +355,46 @@ class ActionServer : public rclcpp::Node return; } + auto jmg = + current_state_debug->getJointModelGroup(param_MOVE_GROUP); + + if (!jmg) + { + RCLCPP_ERROR( + get_logger(), + "JointModelGroup '%s' NOT FOUND", + param_MOVE_GROUP.c_str() + ); + return; + } + + RCLCPP_INFO( + get_logger(), + "JointModelGroup '%s' FOUND", + param_MOVE_GROUP.c_str() + ); + + RCLCPP_INFO( + get_logger(), + "State satisfies bounds = %s", + current_state_debug->satisfiesBounds(jmg) + ? "YES" + : "NO" + ); + + if (!current_state_debug) + { + RCLCPP_ERROR( + get_logger(), + "getCurrentState() returned nullptr" + ); + return; + } + std::vector joints; current_state_debug->copyJointGroupPositions( - current_state_debug->getJointModelGroup(param_MOVE_GROUP), + jmg, joints ); @@ -374,10 +410,9 @@ class ActionServer : public rclcpp::Node bool ik_ok = current_state_debug->setFromIK( - current_state_debug->getJointModelGroup(param_MOVE_GROUP), + jmg, TARGET_POSE, - "tool0", - 5, + std::string("tool0"), 0.1 ); @@ -392,7 +427,7 @@ class ActionServer : public rclcpp::Node std::vector ik_joints; current_state_debug->copyJointGroupPositions( - current_state_debug->getJointModelGroup(param_MOVE_GROUP), + jmg, ik_joints ); @@ -407,6 +442,36 @@ class ActionServer : public rclcpp::Node } } + auto fresh_state = + move_group_interface_ROB.getCurrentState(5.0); + + if (!fresh_state) + { + RCLCPP_ERROR( + get_logger(), + "Fresh state is NULL" + ); + } + else + { + std::vector fresh_joints; + + fresh_state->copyJointGroupPositions( + jmg, + fresh_joints + ); + + for (size_t i = 0; i < fresh_joints.size(); i++) + { + RCLCPP_INFO( + get_logger(), + "Fresh Joint %ld = %.6f", + i + 1, + fresh_joints[i] + ); + } + } + move_group_interface_ROB.setPoseTarget(TARGET_POSE); auto stored_target = From ef4f9c4e239ccf4ec4970ad6ceca0a38d42664e5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 14 Jun 2026 23:59:16 +0200 Subject: [PATCH 22/88] Add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 16097a598..916411144 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -416,6 +416,20 @@ class ActionServer : public rclcpp::Node 0.1 ); + RCLCPP_INFO( + get_logger(), + "IK state satisfies bounds = %s", + current_state_debug->satisfiesBounds() ? "YES" : "NO" + ); + + current_state_debug->enforceBounds(); + + RCLCPP_INFO( + get_logger(), + "After enforceBounds = %s", + current_state_debug->satisfiesBounds() ? "YES" : "NO" + ); + RCLCPP_INFO( get_logger(), "Direct IK result = %s", @@ -472,6 +486,9 @@ class ActionServer : public rclcpp::Node } } + move_group_interface_ROB.clearPoseTargets(); + move_group_interface_ROB.clearPathConstraints(); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); auto stored_target = From 0caa29372bbd8a1cdbbce7285e02a343d480fa7c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 16 Jun 2026 10:33:04 +0200 Subject: [PATCH 23/88] Delete robot stand from xacro --- .../urdf/ur3_robotiq_2f85.urdf.xacro | 47 ++++--------------- 1 file changed, 9 insertions(+), 38 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro index 69935a0a8..c65c22c45 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro @@ -115,44 +115,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -166,4 +128,13 @@ /> + + + + + + + + + From 60ec9f9cfe55d438925cac956087926636583429 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 16 Jun 2026 11:07:34 +0200 Subject: [PATCH 24/88] Add robot stand to world --- Worlds/sausage_exercise.world | 30 ++++++++++++++++++++++++++++++ database/universes.sql | 2 +- 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/Worlds/sausage_exercise.world b/Worlds/sausage_exercise.world index 2528dcdf6..0c1113a48 100644 --- a/Worlds/sausage_exercise.world +++ b/Worlds/sausage_exercise.world @@ -71,6 +71,36 @@ 0 0 0 0 0 0 + + + true + -0.5 0 0 0 0 0 0 + + 0 0 0.45 0 0 0 + + + + 0.09 + 0.9 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + + + + + + 0.09 + 0.9 + + + + + + \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 8a18971a7..8501591ec 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {-1.0,10.0,0.1,0.0,0.0,0.0} 68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 70 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {0.0,0.0,0.9,0.0,0.0,0.0} -71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {-0.5,0.0,0.0,0.0,0.0,0.0} +71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {-0.5,0.0,0.9,0.0,0.0,0.0} \. -- From a909a598d2617e1f65e2cea15bfc00de839c589e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 16 Jun 2026 11:28:46 +0200 Subject: [PATCH 25/88] Fix sausage world --- Worlds/sausage_exercise.world | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Worlds/sausage_exercise.world b/Worlds/sausage_exercise.world index 0c1113a48..886f00a84 100644 --- a/Worlds/sausage_exercise.world +++ b/Worlds/sausage_exercise.world @@ -74,7 +74,7 @@ true - -0.5 0 0 0 0 0 0 + -0.5 0 0 0 0 0 0 0 0.45 0 0 0 From 4fd470a23701961a0dcc13e6442b3884b676e243 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 17 Jun 2026 07:52:03 +0200 Subject: [PATCH 26/88] Add tf to launcher --- CustomRobots/robot_arms/launch/ur3.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index 3cb8cf57d..ab5dce448 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -403,9 +403,9 @@ def launch_setup(context): def generate_launch_description(): declared_arguments = [ - DeclareLaunchArgument("x", default_value="0"), + DeclareLaunchArgument("x", default_value="-0.5"), DeclareLaunchArgument("y", default_value="0"), - DeclareLaunchArgument("z", default_value="0"), + DeclareLaunchArgument("z", default_value="0.9"), DeclareLaunchArgument("R", default_value="0"), DeclareLaunchArgument("P", default_value="0"), DeclareLaunchArgument("Y", default_value="0"), From 34a162f92c7fdbf9558fb1516520a4b4d38f5423 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 17 Jun 2026 08:20:00 +0200 Subject: [PATCH 27/88] add more debug to robmove --- .../ros2srrc_execution/src/robmove.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 916411144..99ea4cbde 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -215,6 +215,12 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.getEndEffectorLink().c_str() ); + RCLCPP_INFO( + get_logger(), + "Pose reference frame = %s", + move_group_interface_ROB.getPoseReferenceFrame().c_str() + ); + auto state = move_group_interface_ROB.getCurrentState(); if (state) @@ -343,6 +349,18 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.getEndEffectorLink().c_str() ); + RCLCPP_INFO( + logger, + "Pose reference frame = %s", + move_group_interface_ROB.getPoseReferenceFrame().c_str() + ); + + RCLCPP_INFO( + logger, + "Default planner = %s", + move_group_interface_ROB.getPlannerId().c_str() + ); + auto current_state_debug = move_group_interface_ROB.getCurrentState(5.0); @@ -649,6 +667,18 @@ int main(int argc, char **argv) RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", ROBname.c_str()); + RCLCPP_INFO( + logger, + "Planning frame = %s", + move_group_interface_ROB.getPlanningFrame().c_str() + ); + + RCLCPP_INFO( + logger, + "End effector link = %s", + move_group_interface_ROB.getEndEffectorLink().c_str() + ); + rclcpp::spin(node); rclcpp::shutdown(); From 4d9ea9f91f22d650356b62fbdde237fac23606de Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 17 Jun 2026 08:34:00 +0200 Subject: [PATCH 28/88] add more debug to robmove --- .../ros2srrc_execution/src/robmove.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 99ea4cbde..6022780bb 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -350,13 +350,13 @@ class ActionServer : public rclcpp::Node ); RCLCPP_INFO( - logger, + get_logger(), "Pose reference frame = %s", move_group_interface_ROB.getPoseReferenceFrame().c_str() ); RCLCPP_INFO( - logger, + get_logger(), "Default planner = %s", move_group_interface_ROB.getPlannerId().c_str() ); From cc443627a1266855c4e9f488ac0de12234814034 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 17 Jun 2026 08:56:51 +0200 Subject: [PATCH 29/88] add rviz launcher --- Launchers/rviz/sausage_exercise.launch.py | 126 ++++++++++++++++++++++ database/universes.sql | 2 +- 2 files changed, 127 insertions(+), 1 deletion(-) create mode 100644 Launchers/rviz/sausage_exercise.launch.py diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py new file mode 100644 index 000000000..4b2e59ed0 --- /dev/null +++ b/Launchers/rviz/sausage_exercise.launch.py @@ -0,0 +1,126 @@ +""" +UR3 + Robotiq - RViz Launcher +Launches ONLY RViz +Assumes Gazebo, robot and move_group are already running +""" + +import os +import xacro +import yaml + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import TimerAction + +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + pkg_path = get_package_share_directory(package_name) + with open(os.path.join(pkg_path, file_path), "r") as f: + return f.read() + + +def load_yaml(package_name, file_path): + pkg_path = get_package_share_directory(package_name) + with open(os.path.join(pkg_path, file_path), "r") as f: + return yaml.safe_load(f) + + +def generate_launch_description(): + + # ===================================================== + # ROBOT DESCRIPTION + # ===================================================== + + xacro_file = os.path.join( + get_package_share_directory("ros2srrc_ur3_gazebo"), + "urdf", + "ur3_robotiq_2f85.urdf.xacro", + ) + + robot_description_content = xacro.process_file( + xacro_file, + mappings={ + "bringup": "false", + "hmi": "false", + "robot_ip": "0.0.0.0", + "EE": "true", + "EE_name": "robotiq_2f85", + "camera": "false", + "script_filename": "none", + "input_recipe_filename": "none", + "output_recipe_filename": "none", + }, + ).toxml() + + robot_description = { + "robot_description": robot_description_content + } + + # ===================================================== + # SRDF + # ===================================================== + + robot_description_semantic = { + "robot_description_semantic": load_file( + "ros2srrc_ur3_moveit2", + "config/ur3robotiq_2f85.srdf", + ) + } + + # ===================================================== + # KINEMATICS + # ===================================================== + + kinematics_yaml = { + "robot_description_kinematics": load_yaml( + "ros2srrc_robots", + "ur3/config/kinematics.yaml", + ) + } + + # ===================================================== + # OMPL + # ===================================================== + + ompl_planning = load_yaml( + "ros2srrc_robots", + "ur3/config/ompl_planning.yaml", + ) + + ompl_planning = ompl_planning["/**"]["ros__parameters"] + + # ===================================================== + # RVIZ CONFIG + # ===================================================== + + rviz_config_file = os.path.join( + get_package_share_directory("ros2srrc_ur3_moveit2"), + "config", + "moveit.rviz", + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning, + {"use_sim_time": True}, + ], + ) + + delay_rviz = TimerAction( + period=3.0, + actions=[rviz_node], + ) + + return LaunchDescription([ + delay_rviz, + ]) \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 8501591ec..9fba468a2 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {-1.0,10.0,0.1,0.0,0.0,0.0} 68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 70 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {0.0,0.0,0.9,0.0,0.0,0.0} -71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py None ROS2 gz {-0.5,0.0,0.9,0.0,0.0,0.0} +71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz {-0.5,0.0,0.9,0.0,0.0,0.0} \. -- From 94bc1cf920e6acacae976a9273a873609cd83b9d Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 17 Jun 2026 09:20:22 +0200 Subject: [PATCH 30/88] Fix rviz launcher --- Launchers/rviz/sausage_exercise.launch.py | 87 ++++++++++++----------- 1 file changed, 44 insertions(+), 43 deletions(-) diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py index 4b2e59ed0..3e3465707 100644 --- a/Launchers/rviz/sausage_exercise.launch.py +++ b/Launchers/rviz/sausage_exercise.launch.py @@ -1,12 +1,11 @@ """ UR3 + Robotiq - RViz Launcher -Launches ONLY RViz -Assumes Gazebo, robot and move_group are already running +Launches ONLY RViz with MoveIt Motion Planning +Assumes Gazebo and robot are already running """ import os import xacro -import yaml from launch import LaunchDescription from launch_ros.actions import Node @@ -15,26 +14,26 @@ from ament_index_python.packages import get_package_share_directory -def load_file(package_name, file_path): - pkg_path = get_package_share_directory(package_name) - with open(os.path.join(pkg_path, file_path), "r") as f: - return f.read() - +def generate_launch_description(): -def load_yaml(package_name, file_path): - pkg_path = get_package_share_directory(package_name) - with open(os.path.join(pkg_path, file_path), "r") as f: - return yaml.safe_load(f) + # ===================================================== + # Packages + # ===================================================== + moveit_pkg_share = get_package_share_directory( + "ros2srrc_ur3_moveit2" + ) -def generate_launch_description(): + gazebo_pkg_share = get_package_share_directory( + "ros2srrc_ur3_gazebo" + ) # ===================================================== - # ROBOT DESCRIPTION + # URDF # ===================================================== xacro_file = os.path.join( - get_package_share_directory("ros2srrc_ur3_gazebo"), + gazebo_pkg_share, "urdf", "ur3_robotiq_2f85.urdf.xacro", ) @@ -62,42 +61,42 @@ def generate_launch_description(): # SRDF # ===================================================== - robot_description_semantic = { - "robot_description_semantic": load_file( - "ros2srrc_ur3_moveit2", - "config/ur3robotiq_2f85.srdf", - ) - } - - # ===================================================== - # KINEMATICS - # ===================================================== + srdf_file = os.path.join( + moveit_pkg_share, + "config", + "ur3robotiq_2f85.srdf", + ) - kinematics_yaml = { - "robot_description_kinematics": load_yaml( - "ros2srrc_robots", - "ur3/config/kinematics.yaml", - ) - } + with open(srdf_file, "r") as file: + robot_description_semantic = { + "robot_description_semantic": file.read() + } # ===================================================== - # OMPL + # MoveIt Config Files # ===================================================== - ompl_planning = load_yaml( - "ros2srrc_robots", - "ur3/config/ompl_planning.yaml", + kinematics_yaml = os.path.join( + get_package_share_directory("ros2srrc_robots"), + "ur3", + "config", + "kinematics.yaml", ) - ompl_planning = ompl_planning["/**"]["ros__parameters"] + ompl_planning_yaml = os.path.join( + get_package_share_directory("ros2srrc_robots"), + "ur3", + "config", + "ompl_planning.yaml", + ) # ===================================================== - # RVIZ CONFIG + # RViz Config # ===================================================== rviz_config_file = os.path.join( - get_package_share_directory("ros2srrc_ur3_moveit2"), - "config", + moveit_pkg_share, + "rviz", "moveit.rviz", ) @@ -110,8 +109,8 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, + ompl_planning_yaml, kinematics_yaml, - ompl_planning, {"use_sim_time": True}, ], ) @@ -121,6 +120,8 @@ def generate_launch_description(): actions=[rviz_node], ) - return LaunchDescription([ - delay_rviz, - ]) \ No newline at end of file + return LaunchDescription( + [ + delay_rviz, + ] + ) \ No newline at end of file From d3678415d82456dbec1427917a5f7b5de118ad51 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 18 Jun 2026 07:23:49 +0200 Subject: [PATCH 31/88] Add Rviz launcher --- .../ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz | 311 ++++++++++++++++++ Launchers/rviz/sausage_exercise.launch.py | 31 +- 2 files changed, 330 insertions(+), 12 deletions(-) create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz new file mode 100644 index 000000000..c8092de74 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz @@ -0,0 +1,311 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.4957627058029175 + Tree Height: 573 + - 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: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: true + Interrupt Display: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.3 + Joint Violation Color: 255; 0; 255 + Planning Group: ur3_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + 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.009999999776482582 + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + 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: 6.619869709014893 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.2489434778690338 + Y: -0.013962505385279655 + Z: 0.13800443708896637 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4103981554508209 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.210397720336914 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1381 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e0000004f6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000002e3000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032a00000212000001b9010000030000000100000110000004f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000046000004f6000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000070e000004f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 30 diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py index 3e3465707..0773dc11e 100644 --- a/Launchers/rviz/sausage_exercise.launch.py +++ b/Launchers/rviz/sausage_exercise.launch.py @@ -6,6 +6,7 @@ import os import xacro +import yaml from launch import LaunchDescription from launch_ros.actions import Node @@ -13,6 +14,11 @@ from ament_index_python.packages import get_package_share_directory +def load_yaml(package_name, file_path): + pkg_path = get_package_share_directory(package_name) + with open(os.path.join(pkg_path, file_path), "r") as f: + return yaml.safe_load(f) + def generate_launch_description(): @@ -23,6 +29,7 @@ def generate_launch_description(): moveit_pkg_share = get_package_share_directory( "ros2srrc_ur3_moveit2" ) + gazebo_pkg_share = get_package_share_directory( "ros2srrc_ur3_gazebo" @@ -76,20 +83,20 @@ def generate_launch_description(): # MoveIt Config Files # ===================================================== - kinematics_yaml = os.path.join( - get_package_share_directory("ros2srrc_robots"), - "ur3", - "config", - "kinematics.yaml", - ) + kinematics_yaml = { + "robot_description_kinematics": load_yaml( + "ros2srrc_robots", + "ur3/config/kinematics.yaml", + ) + } - ompl_planning_yaml = os.path.join( - get_package_share_directory("ros2srrc_robots"), - "ur3", - "config", - "ompl_planning.yaml", + ompl_planning = load_yaml( + "ros2srrc_robots", + "ur3/config/ompl_planning.yaml", ) + ompl_planning = ompl_planning["/**"]["ros__parameters"] + # ===================================================== # RViz Config # ===================================================== @@ -109,7 +116,7 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, - ompl_planning_yaml, + ompl_planning, kinematics_yaml, {"use_sim_time": True}, ], From 96cc7f4d665d8efefa4ec8ebdb29d2783f4a3a31 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 18 Jun 2026 08:13:55 +0200 Subject: [PATCH 32/88] Add moveit.rviz to ur3 --- .../packages/{ur5/ros2srrc_ur5_moveit2 => ur3}/rviz/moveit.rviz | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Industrial/ros2_SimRealRobotControl_gz/packages/{ur5/ros2srrc_ur5_moveit2 => ur3}/rviz/moveit.rviz (100%) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/rviz/moveit.rviz similarity index 100% rename from Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/rviz/moveit.rviz rename to Industrial/ros2_SimRealRobotControl_gz/packages/ur3/rviz/moveit.rviz From 12429cbcd5b899d914812e6995ea2843ae0573dd Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 18 Jun 2026 08:27:48 +0200 Subject: [PATCH 33/88] Change repo for moveit.rviz --- .../packages/ur3/{ => ros2srrc_ur3_moveit2}/rviz/moveit.rviz | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Industrial/ros2_SimRealRobotControl_gz/packages/ur3/{ => ros2srrc_ur3_moveit2}/rviz/moveit.rviz (100%) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/rviz/moveit.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz similarity index 100% rename from Industrial/ros2_SimRealRobotControl_gz/packages/ur3/rviz/moveit.rviz rename to Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz From 1a5d3cdbaee243af417211e6e1e10e7434e81a98 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 18 Jun 2026 08:57:29 +0200 Subject: [PATCH 34/88] Change path for rviz moveit.rviz --- Launchers/rviz/sausage_exercise.launch.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py index 0773dc11e..3c3af64b3 100644 --- a/Launchers/rviz/sausage_exercise.launch.py +++ b/Launchers/rviz/sausage_exercise.launch.py @@ -101,12 +101,14 @@ def generate_launch_description(): # RViz Config # ===================================================== - rviz_config_file = os.path.join( - moveit_pkg_share, - "rviz", - "moveit.rviz", + rviz_config_file = ( + "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/" + "packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz" ) + print("MOVEIT PKG =", moveit_pkg_share) + print("RVIZ FILE =", rviz_config_file) + rviz_node = Node( package="rviz2", executable="rviz2", From f64d22e0cff1cc916b462bd618f00208278dca21 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 18 Jun 2026 10:02:01 +0200 Subject: [PATCH 35/88] Try to fix planification error --- CustomRobots/robot_arms/launch/ur3.launch.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index ab5dce448..af5f396ad 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -171,18 +171,21 @@ def launch_setup(context): } planning_pipelines_config = { - "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], - "default_planning_pipeline": "pilz_industrial_motion_planner", + "planning_pipelines": ["ompl"], + "default_planning_pipeline": "ompl", "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - }, - "pilz_industrial_motion_planner": { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - "request_adapters": "", + "request_adapters": + "default_planner_request_adapters/AddTimeOptimalParameterization " + "default_planner_request_adapters/ResolveConstraintFrames " + "default_planner_request_adapters/FixWorkspaceBounds " + "default_planner_request_adapters/FixStartStateBounds " + "default_planner_request_adapters/FixStartStateCollision " + "default_planner_request_adapters/FixStartStatePathConstraints", + "start_state_max_bounds_error": 0.1, - "default_planner_config": "PTP", }, } From 06378d002e98757ca1bc40e097b4a42a0132fdce Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 10:54:11 +0200 Subject: [PATCH 36/88] Change nodes to be parameterizable --- .../ros2srrc_execution/src/move.cpp | 56 +-- .../ros2srrc_execution/src/robmove.cpp | 438 +----------------- .../ros2srrc_execution/src/robpose.cpp | 72 +-- 3 files changed, 49 insertions(+), 517 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp index e09c50a21..cafec7d0f 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp @@ -76,7 +76,7 @@ std::string param_ROB = "none"; std::string param_EE = "none"; std::string param_ENV = "none"; -std::string param_MOVE_GROUP = "ur5_manipulator"; +std::string param_ROB_GROUP = "none"; // Declaration of GLOBAL VARIABLES --> MoveIt!2 Interface: moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; @@ -99,13 +99,22 @@ ros2srrc_data::msg::Specs eeSPECS; class ros2_RobotParam : public rclcpp::Node { public: - ros2_RobotParam() : Node("ros2_RobotParam") + ros2_RobotParam() : Node("ros2_RobotParam") { this->declare_parameter("ROB_PARAM", "none"); - param_ROB = this->get_parameter("ROB_PARAM").get_parameter_value().get(); - RCLCPP_INFO(this->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str()); + param_ROB = this->get_parameter("ROB_PARAM").as_string(); + + this->declare_parameter("ROB_GROUP", "none"); + param_ROB_GROUP = this->get_parameter("ROB_GROUP").as_string(); + + RCLCPP_INFO(this->get_logger(), + "ROB_PARAM received -> %s", + param_ROB.c_str()); + + RCLCPP_INFO(this->get_logger(), + "ROB_GROUP received -> %s", + param_ROB_GROUP.c_str()); } -private: }; class ros2_EEParam : public rclcpp::Node @@ -120,29 +129,6 @@ class ros2_EEParam : public rclcpp::Node private: }; -class ros2_MoveGroupParam : public rclcpp::Node -{ -public: - ros2_MoveGroupParam() : Node("ros2_MoveGroupParam") - { - this->declare_parameter( - "MOVE_GROUP", - "ur5_manipulator" - ); - - param_MOVE_GROUP = - this->get_parameter("MOVE_GROUP") - .get_parameter_value() - .get(); - - RCLCPP_INFO( - this->get_logger(), - "MOVE_GROUP received -> %s", - param_MOVE_GROUP.c_str() - ); - } -}; - // ======================================================================================================================== // // ==================== FUNCTIONS ==================== // @@ -426,7 +412,7 @@ class ActionServer : public rclcpp::Node robot_trajectory::RobotTrajectory rt( move_group_interface_ROB.getRobotModel(), - param_MOVE_GROUP + param_ROB_GROUP ); moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); @@ -518,9 +504,6 @@ int main(int argc, char ** argv) auto node_PARAM_EE = std::make_shared(); rclcpp::spin_some(node_PARAM_EE); - auto node_PARAM_MOVE_GROUP = std::make_shared(); - rclcpp::spin_some(node_PARAM_MOVE_GROUP); - // ================= LOAD SPECS ================= if (param_ROB != "none"){ std::string pkgPATH_R = @@ -586,9 +569,8 @@ int main(int argc, char ** argv) // ROBOT if (param_ROB != "none"){ - std::string group_name = param_MOVE_GROUP; - - move_group_interface_ROB = MoveGroupInterface(node2, group_name); + move_group_interface_ROB = + MoveGroupInterface(node2, param_ROB_GROUP); move_group_interface_ROB.setMaxVelocityScalingFactor(0.5); move_group_interface_ROB.setMaxAccelerationScalingFactor(0.5); @@ -599,11 +581,11 @@ int main(int argc, char ** argv) joint_model_group_ROB = move_group_interface_ROB.getCurrentState() - ->getJointModelGroup(group_name); + ->getJointModelGroup(param_ROB_GROUP); RCLCPP_INFO(logger, "MoveGroupInterface created for ROBOT group: %s", - group_name.c_str()); + param_ROB_GROUP.c_str()); } // END EFFECTOR diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 6022780bb..02b7a18d7 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -52,83 +52,33 @@ moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; // Declaration of GLOBAL VARIABLE --> ROBOT PARAMETER: std::string param_ROB = "none"; -std::string param_MOVE_GROUP = "ur5_manipulator"; // Declaration of GLOBAL VARIABLE --> RES: std::string RES = "none"; +std::string param_ROB_GROUP = "none"; + // =============================================================================== // // MoveIt!2 -> MoveGroupInterface/Plan function: -moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() -{ - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - - RCLCPP_INFO( - rclcpp::get_logger("PLAN_DEBUG"), - "Pose target frame = %s", - move_group_interface_ROB.getPoseTarget().header.frame_id.c_str() - ); - - auto cp = move_group_interface_ROB.getCurrentPose(); - - RCLCPP_INFO( - rclcpp::get_logger("PLAN_DEBUG"), - "Current pose = %.3f %.3f %.3f", - cp.pose.position.x, - cp.pose.position.y, - cp.pose.position.z - ); - - auto result = move_group_interface_ROB.plan(my_plan); - - RCLCPP_INFO( - rclcpp::get_logger("PLAN_DEBUG"), - "MoveIt plan result code = %d", - result.val - ); - - bool success = - (result == moveit::planning_interface::MoveItErrorCode::SUCCESS); +moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() { - if (!success) - { - switch(result.val) - { - case -31: - RCLCPP_ERROR( - rclcpp::get_logger("PLAN_DEBUG"), - "NO IK SOLUTION (-31)" - ); - break; - - case -2: - RCLCPP_ERROR( - rclcpp::get_logger("PLAN_DEBUG"), - "INVALID MOTION PLAN (-2)" - ); - break; - - default: - RCLCPP_ERROR( - rclcpp::get_logger("PLAN_DEBUG"), - "ERROR CODE = %d", - result.val - ); - } - } + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_interface_ROB.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + // Execute the plan if (success) { RES = "PLANNING: OK"; + return(my_plan); } else { RES = "PLANNING: ERROR"; + return(my_plan); } - return my_plan; -} +}; // =============================================================================== // // ROS2 Action Server to move the ROBOT: @@ -143,24 +93,13 @@ class ActionServer : public rclcpp::Node explicit ActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("ros2srrc_RobMove", options){ this->declare_parameter("ROB_PARAM", "none"); - - this->declare_parameter( - "MOVE_GROUP", - "ur5_manipulator" - ); - - param_MOVE_GROUP = - this->get_parameter("MOVE_GROUP").as_string(); - - RCLCPP_INFO( - this->get_logger(), - "MOVE_GROUP received -> %s", - param_MOVE_GROUP.c_str() - ); - param_ROB = this->get_parameter("ROB_PARAM").as_string(); RCLCPP_INFO(this->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str()); + this->declare_parameter("ROB_GROUP", "none"); + param_ROB_GROUP = + this->get_parameter("ROB_GROUP").as_string(); + action_server_ = rclcpp_action::create_server( this, "/Robmove", @@ -202,76 +141,13 @@ class ActionServer : public rclcpp::Node void execute(const std::shared_ptr goal_handle) { - // ================= DEBUG ================= - RCLCPP_INFO( - get_logger(), - "Planning frame: %s", - move_group_interface_ROB.getPlanningFrame().c_str() - ); - - RCLCPP_INFO( - get_logger(), - "End effector link: %s", - move_group_interface_ROB.getEndEffectorLink().c_str() - ); - - RCLCPP_INFO( - get_logger(), - "Pose reference frame = %s", - move_group_interface_ROB.getPoseReferenceFrame().c_str() - ); - - auto state = move_group_interface_ROB.getCurrentState(); - - if (state) - { - const Eigen::Isometry3d& t = - state->getGlobalLinkTransform("tool0"); - - RCLCPP_INFO( - get_logger(), - "tool0 RobotState -> x=%.3f y=%.3f z=%.3f", - t.translation().x(), - t.translation().y(), - t.translation().z() - ); - } - - auto cp_tool0 = - move_group_interface_ROB.getCurrentPose("tool0"); - - auto cp_ee = - move_group_interface_ROB.getCurrentPose( - move_group_interface_ROB.getEndEffectorLink() - ); - - RCLCPP_INFO( - get_logger(), - "tool0 -> %.3f %.3f %.3f", - cp_tool0.pose.position.x, - cp_tool0.pose.position.y, - cp_tool0.pose.position.z - ); - - RCLCPP_INFO( - get_logger(), - "EEF -> %.3f %.3f %.3f", - cp_ee.pose.position.x, - cp_ee.pose.position.y, - cp_ee.pose.position.z - ); + // 0. INFORMATION -> Current Robot Pose: auto CP_INFO = move_group_interface_ROB.getCurrentPose(); + RCLCPP_INFO(get_logger(), "INFORMATION -> Current Robot Pose:"); + RCLCPP_INFO(get_logger(), "POSITION -> (x: %.3f, y: %.3f, z: %.3f)", CP_INFO.pose.position.x, CP_INFO.pose.position.y, CP_INFO.pose.position.z); + RCLCPP_INFO(get_logger(), "ORIENTATION -> (qx: %.3f, qy: %.3f, qz: %.3f, qw: %.3f)", CP_INFO.pose.orientation.x, CP_INFO.pose.orientation.y, CP_INFO.pose.orientation.z, CP_INFO.pose.orientation.w); - RCLCPP_INFO( - get_logger(), - "Default CurrentPose -> x=%.3f y=%.3f z=%.3f", - CP_INFO.pose.position.x, - CP_INFO.pose.position.y, - CP_INFO.pose.position.z - ); - - // ================= FIN DEBUG ================= // 1. OBTAIN INPUT PARAMETERS: const auto GOAL = goal_handle->get_goal(); @@ -284,25 +160,6 @@ class ActionServer : public rclcpp::Node auto CURRENT_POSE = move_group_interface_ROB.getCurrentPose(); - auto CURRENT_POSE_TOOL0 = - move_group_interface_ROB.getCurrentPose("tool0"); - - RCLCPP_INFO( - get_logger(), - "CURRENT_POSE(default) -> %.6f %.6f %.6f", - CURRENT_POSE.pose.position.x, - CURRENT_POSE.pose.position.y, - CURRENT_POSE.pose.position.z - ); - - RCLCPP_INFO( - get_logger(), - "CURRENT_POSE(tool0) -> %.6f %.6f %.6f", - CURRENT_POSE_TOOL0.pose.position.x, - CURRENT_POSE_TOOL0.pose.position.y, - CURRENT_POSE_TOOL0.pose.position.z - ); - geometry_msgs::msg::Pose TARGET_POSE; TARGET_POSE.position.x = GOAL->x; TARGET_POSE.position.y = GOAL->y; @@ -312,248 +169,19 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z = GOAL->qz; TARGET_POSE.orientation.w = GOAL->qw; - RCLCPP_INFO( - get_logger(), - "TARGET -> x=%.3f y=%.3f z=%.3f", - TARGET_POSE.position.x, - TARGET_POSE.position.y, - TARGET_POSE.position.z - ); - - RCLCPP_INFO( - get_logger(), - "TARGET Q -> %.3f %.3f %.3f %.3f", - TARGET_POSE.orientation.x, - TARGET_POSE.orientation.y, - TARGET_POSE.orientation.z, - TARGET_POSE.orientation.w - ); - - RCLCPP_INFO( - get_logger(), - "Current pose before planning -> x=%.6f y=%.6f z=%.6f", - CURRENT_POSE.pose.position.x, - CURRENT_POSE.pose.position.y, - CURRENT_POSE.pose.position.z - ); - - RCLCPP_INFO( - get_logger(), - "Current pose frame -> %s", - CURRENT_POSE.header.frame_id.c_str() - ); - - RCLCPP_INFO( - get_logger(), - "End effector link = %s", - move_group_interface_ROB.getEndEffectorLink().c_str() - ); - - RCLCPP_INFO( - get_logger(), - "Pose reference frame = %s", - move_group_interface_ROB.getPoseReferenceFrame().c_str() - ); - - RCLCPP_INFO( - get_logger(), - "Default planner = %s", - move_group_interface_ROB.getPlannerId().c_str() - ); - - auto current_state_debug = - move_group_interface_ROB.getCurrentState(5.0); - - if (!current_state_debug) - { - RCLCPP_ERROR( - get_logger(), - "getCurrentState() returned nullptr" - ); - return; - } - - auto jmg = - current_state_debug->getJointModelGroup(param_MOVE_GROUP); - - if (!jmg) - { - RCLCPP_ERROR( - get_logger(), - "JointModelGroup '%s' NOT FOUND", - param_MOVE_GROUP.c_str() - ); - return; - } - - RCLCPP_INFO( - get_logger(), - "JointModelGroup '%s' FOUND", - param_MOVE_GROUP.c_str() - ); - - RCLCPP_INFO( - get_logger(), - "State satisfies bounds = %s", - current_state_debug->satisfiesBounds(jmg) - ? "YES" - : "NO" - ); - - if (!current_state_debug) - { - RCLCPP_ERROR( - get_logger(), - "getCurrentState() returned nullptr" - ); - return; - } - - std::vector joints; - - current_state_debug->copyJointGroupPositions( - jmg, - joints - ); - - for (size_t i = 0; i < joints.size(); i++) - { - RCLCPP_INFO( - get_logger(), - "Joint %ld = %.6f", - i + 1, - joints[i] - ); - } - - bool ik_ok = - current_state_debug->setFromIK( - jmg, - TARGET_POSE, - std::string("tool0"), - 0.1 - ); - - RCLCPP_INFO( - get_logger(), - "IK state satisfies bounds = %s", - current_state_debug->satisfiesBounds() ? "YES" : "NO" - ); - - current_state_debug->enforceBounds(); - - RCLCPP_INFO( - get_logger(), - "After enforceBounds = %s", - current_state_debug->satisfiesBounds() ? "YES" : "NO" - ); - - RCLCPP_INFO( - get_logger(), - "Direct IK result = %s", - ik_ok ? "SUCCESS" : "FAILED" - ); - - if (ik_ok) - { - std::vector ik_joints; - - current_state_debug->copyJointGroupPositions( - jmg, - ik_joints - ); - - for (size_t i = 0; i < ik_joints.size(); i++) - { - RCLCPP_INFO( - get_logger(), - "IK Joint %ld = %.6f", - i + 1, - ik_joints[i] - ); - } - } - - auto fresh_state = - move_group_interface_ROB.getCurrentState(5.0); - - if (!fresh_state) - { - RCLCPP_ERROR( - get_logger(), - "Fresh state is NULL" - ); - } - else - { - std::vector fresh_joints; - - fresh_state->copyJointGroupPositions( - jmg, - fresh_joints - ); - - for (size_t i = 0; i < fresh_joints.size(); i++) - { - RCLCPP_INFO( - get_logger(), - "Fresh Joint %ld = %.6f", - i + 1, - fresh_joints[i] - ); - } - } - - move_group_interface_ROB.clearPoseTargets(); - move_group_interface_ROB.clearPathConstraints(); - move_group_interface_ROB.setPoseTarget(TARGET_POSE); - auto stored_target = - move_group_interface_ROB.getPoseTarget(); - - RCLCPP_INFO( - get_logger(), - "Stored target -> x=%.6f y=%.6f z=%.6f", - stored_target.pose.position.x, - stored_target.pose.position.y, - stored_target.pose.position.z - ); - - RCLCPP_INFO( - get_logger(), - "Stored target frame -> %s", - stored_target.header.frame_id.c_str() - ); - - move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed); - /* NUEVO DEBUG */ - move_group_interface_ROB.setPlanningTime(10.0); - - RCLCPP_INFO( - get_logger(), - "Planner ID = %s", - move_group_interface_ROB.getPlannerId().c_str() - ); - - /* PLANIFICAR */ MyPlan = plan_ROB(); - /* NUEVO DEBUG */ - RCLCPP_INFO( - get_logger(), - "Planning result = %s", - RES.c_str() - ); - if (RES == "PLANNING: OK"){ robot_trajectory::RobotTrajectory rt( move_group_interface_ROB.getRobotModel(), - param_MOVE_GROUP + param_ROB_GROUP ); moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); @@ -649,35 +277,13 @@ int main(int argc, char **argv) using moveit::planning_interface::MoveGroupInterface; - RCLCPP_INFO( - logger, - "param_MOVE_GROUP = %s", - param_MOVE_GROUP.c_str() - ); - - std::string ROBname = param_MOVE_GROUP; - - move_group_interface_ROB = MoveGroupInterface(moveit_node, ROBname); - - move_group_interface_ROB.startStateMonitor(); - rclcpp::sleep_for(std::chrono::seconds(2)); + move_group_interface_ROB = + MoveGroupInterface(moveit_node, param_ROB_GROUP); move_group_interface_ROB.setMaxVelocityScalingFactor(1.0); move_group_interface_ROB.setMaxAccelerationScalingFactor(1.0); - RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", ROBname.c_str()); - - RCLCPP_INFO( - logger, - "Planning frame = %s", - move_group_interface_ROB.getPlanningFrame().c_str() - ); - - RCLCPP_INFO( - logger, - "End effector link = %s", - move_group_interface_ROB.getEndEffectorLink().c_str() - ); + RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", param_ROB_GROUP.c_str()); rclcpp::spin(node); diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp index 4eab29e43..fc7b462d5 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp @@ -52,7 +52,8 @@ moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; // Declaration of GLOBAL VARIABLE --> ROBOT PARAMETER: std::string param_ROB = "none"; -std::string param_MOVE_GROUP = "ur5_manipulator"; + +std::string param_ROB_GROUP = "none"; // Declaration of GLOBAL VARIABLE --> ROBOT POSE: ros2srrc_data::msg::Robpose POSE; @@ -67,7 +68,7 @@ class RobPose_PUB : public rclcpp::Node : Node("ros2srrc_RobPosePUB"), count_(0) { publisher_ = this->create_publisher("Robpose", 10); - timer_ = this->create_wall_timer(2000ms, std::bind(&RobPose_PUB::timer_callback, this)); + timer_ = this->create_wall_timer(50ms, std::bind(&RobPose_PUB::timer_callback, this)); } private: @@ -80,29 +81,6 @@ class RobPose_PUB : public rclcpp::Node auto CP_INFO = move_group_interface_ROB.getCurrentPose(); - RCLCPP_INFO( - this->get_logger(), - "CurrentPose frame_id = %s", - CP_INFO.header.frame_id.c_str() - ); - - RCLCPP_INFO( - this->get_logger(), - "MoveIt Pose -> x=%.6f y=%.6f z=%.6f", - CP_INFO.pose.position.x, - CP_INFO.pose.position.y, - CP_INFO.pose.position.z - ); - - RCLCPP_INFO( - this->get_logger(), - "MoveIt Quat -> qx=%.6f qy=%.6f qz=%.6f qw=%.6f", - CP_INFO.pose.orientation.x, - CP_INFO.pose.orientation.y, - CP_INFO.pose.orientation.z, - CP_INFO.pose.orientation.w - ); - POSE.x = CP_INFO.pose.position.x; POSE.y = CP_INFO.pose.position.y; POSE.z = CP_INFO.pose.position.z; @@ -133,55 +111,21 @@ int main(int argc, char **argv) // === PARAM === node->declare_parameter("ROB_PARAM", "none"); - node->declare_parameter( - "MOVE_GROUP", - "ur5_manipulator" - ); - param_ROB = node->get_parameter("ROB_PARAM").as_string(); - param_MOVE_GROUP = node->get_parameter("MOVE_GROUP").as_string(); - RCLCPP_INFO(node->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str()); + node->declare_parameter("ROB_GROUP", "none"); + param_ROB_GROUP = + node->get_parameter("ROB_GROUP").as_string(); + // === MOVEIT === using moveit::planning_interface::MoveGroupInterface; - std::string ROBname = param_MOVE_GROUP; - - auto moveit_node = std::make_shared( - "moveit_helper_node_robpose", - rclcpp::NodeOptions() - .automatically_declare_parameters_from_overrides(true) - ); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(moveit_node); - - std::thread([&executor]() { - executor.spin(); - }).detach(); - move_group_interface_ROB = - MoveGroupInterface(moveit_node, ROBname); - - move_group_interface_ROB.startStateMonitor(); - - rclcpp::sleep_for(std::chrono::seconds(2)); + MoveGroupInterface(node, param_ROB_GROUP); RCLCPP_INFO(node->get_logger(), "MoveGroupInterface created"); - RCLCPP_INFO( - node->get_logger(), - "End effector link: %s", - move_group_interface_ROB.getEndEffectorLink().c_str() - ); - - RCLCPP_INFO( - node->get_logger(), - "Planning frame: %s", - move_group_interface_ROB.getPlanningFrame().c_str() - ); - rclcpp::spin(node); rclcpp::shutdown(); From 380476f0b62ba82a8581ce1ddd99707f38ded725 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 11:10:47 +0200 Subject: [PATCH 37/88] change parameter name for nodes in launcher --- CustomRobots/robot_arms/launch/ur3.launch.py | 6 +++--- CustomRobots/robot_arms/launch/ur5.launch.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index af5f396ad..621f9b504 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -218,7 +218,7 @@ def launch_setup(context): {"use_sim_time": True}, {"ROB_PARAM": "ur3"}, {"EE_PARAM": "robotiq_2f85"}, - {"MOVE_GROUP": "ur3_arm"}, + {"ROB_GROUP": "ur3_arm"}, ], ) @@ -239,7 +239,7 @@ def launch_setup(context): }, {"use_sim_time": True}, {"ROB_PARAM": "ur3"}, - {"MOVE_GROUP": "ur3_arm"}, + {"ROB_GROUP": "ur3_arm"}, ], ) @@ -255,7 +255,7 @@ def launch_setup(context): ompl_planning, {"use_sim_time": True}, {"ROB_PARAM": "ur3"}, - {"MOVE_GROUP": "ur3_arm"}, + {"ROB_GROUP": "ur3_arm"}, ], ) diff --git a/CustomRobots/robot_arms/launch/ur5.launch.py b/CustomRobots/robot_arms/launch/ur5.launch.py index 1b7c76e92..b18b06b15 100644 --- a/CustomRobots/robot_arms/launch/ur5.launch.py +++ b/CustomRobots/robot_arms/launch/ur5.launch.py @@ -154,7 +154,7 @@ def launch_setup(context): {"use_sim_time": True}, {"ROB_PARAM": "ur5"}, {"EE_PARAM": "robotiq_2f85"}, - {"MOVE_GROUP": "ur5_manipulator"}, + {"ROB_GROUP": "ur5_manipulator"}, ], ) @@ -174,7 +174,7 @@ def launch_setup(context): }, {"use_sim_time": True}, {"ROB_PARAM": "ur5"}, - {"MOVE_GROUP": "ur5_manipulator"}, + {"ROB_GROUP": "ur5_manipulator"}, ], ) @@ -190,7 +190,7 @@ def launch_setup(context): ompl_planning, {"use_sim_time": True}, {"ROB_PARAM": "ur5"}, - {"MOVE_GROUP": "ur5_manipulator"}, + {"ROB_GROUP": "ur5_manipulator"}, ], ) From 1cb68cd3eeb2cc06a928fa0fe769a7aa16b0a069 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 17:15:17 +0200 Subject: [PATCH 38/88] Change ur3 stack to use ur5 stack --- .../robot_arms/config/ur3_controllers.yaml | 86 +++++ CustomRobots/robot_arms/launch/ur3.launch.py | 169 +++------- .../robot_arms/models/ur3/inc/ur_common.xacro | 193 +++++++++++ .../models/ur3/inc/ur_transmissions.xacro | 73 +++++ .../models/ur3/ur.ros2_control.xacro | 304 ++++++++++++++++++ .../robot_arms/models/ur3/ur.urdf.xacro | 83 +++++ .../robot_arms/models/ur3/ur3.urdf.xacro | 60 ++++ .../robot_arms/models/ur3/ur_generated.urdf | 0 .../robot_arms/models/ur3/ur_macro.xacro | 284 ++++++++++++++++ .../ur3_gripper_moveit_config /CMakeLists.txt | 31 ++ .../config/controllers.yaml | 26 ++ .../config/kinematics.yaml | 9 + .../config/moveit_controllers.yaml | 25 ++ .../config/ompl_planning.yaml | 72 +++++ .../config/ur_servo.yaml | 76 +++++ .../ur3_gripper_moveit_config /package.xml | 32 ++ 16 files changed, 1404 insertions(+), 119 deletions(-) create mode 100644 CustomRobots/robot_arms/config/ur3_controllers.yaml create mode 100644 CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro create mode 100644 CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro create mode 100644 CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro create mode 100644 CustomRobots/robot_arms/models/ur3/ur.urdf.xacro create mode 100644 CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro create mode 100644 CustomRobots/robot_arms/models/ur3/ur_generated.urdf create mode 100644 CustomRobots/robot_arms/models/ur3/ur_macro.xacro create mode 100644 Industrial/ur3_gripper_moveit_config /CMakeLists.txt create mode 100644 Industrial/ur3_gripper_moveit_config /config/controllers.yaml create mode 100644 Industrial/ur3_gripper_moveit_config /config/kinematics.yaml create mode 100644 Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml create mode 100644 Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml create mode 100644 Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml create mode 100644 Industrial/ur3_gripper_moveit_config /package.xml diff --git a/CustomRobots/robot_arms/config/ur3_controllers.yaml b/CustomRobots/robot_arms/config/ur3_controllers.yaml new file mode 100644 index 000000000..0f2d6d906 --- /dev/null +++ b/CustomRobots/robot_arms/config/ur3_controllers.yaml @@ -0,0 +1,86 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_state_broadcaster: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + - robotiq_85_left_knuckle_joint + + interfaces: + - position + - velocity + - effort + +joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + allow_nonzero_velocity_at_trajectory_end: true + allow_integration_in_goal_trajectories: true + + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + +gripper_controller: + ros__parameters: + joints: + - robotiq_85_left_knuckle_joint + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + allow_integration_in_goal_trajectories: true + + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + robotiq_85_left_knuckle_joint: + trajectory: 0.5 + goal: 0.5 + + gains: + robotiq_85_left_knuckle_joint: + p: 100.0 + d: 1.0 + i: 0.01 + i_clamp: 1.0 diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index 621f9b504..7c5aa7cd3 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -31,16 +31,13 @@ def launch_setup(context): x = LaunchConfiguration("x") y = LaunchConfiguration("y") z = LaunchConfiguration("z") - - print("X =", x.perform(context)) - print("Y =", y.perform(context)) - print("Z =", z.perform(context)) - R = LaunchConfiguration("R") P = LaunchConfiguration("P") Y = LaunchConfiguration("Y") gz_sensor = LaunchConfiguration("sensor") + package_dir = get_package_share_directory("custom_robots") + sensor = gz_sensor.perform(context) nodes = [] @@ -49,152 +46,93 @@ def launch_setup(context): # ROBOT DESCRIPTION (URDF) # ========================= xacro_file = os.path.join( - get_package_share_directory("ros2srrc_ur3_gazebo"), - "urdf", - "ur3_robotiq_2f85.urdf.xacro", + package_dir, + "models", + "ur3", + "ur3.urdf.xacro", ) + controllers_file = os.path.join(package_dir, "config", "ur3_controllers.yaml") + robot_description_content = xacro.process_file( xacro_file, mappings={ - "bringup": "false", + "ur_type": "ur3", + "name": "ur", + "prefix": "", + "use_fake_hardware": "false", + "sim_gazebo": "false", + "sim_gz": "true", + "simulation_controllers": controllers_file, "hmi": "false", - "robot_ip": "0.0.0.0", "EE": "true", "EE_name": "robotiq_2f85", "camera": "true" if sensor == "camera" else "false", - "script_filename": "none", - "input_recipe_filename": "none", - "output_recipe_filename": "none", }, ).toxml() robot_description = {"robot_description": robot_description_content} - # ========================= - # ENVIROMENT - # ========================= - - gz_ros2_control_path = "/home/ws/install/gz_ros2_control/lib" - gz_link_attacher_path = "/home/ws/install/gz_link_attacher/lib" - - gz_plugin_path = ( - gz_link_attacher_path + ":" + gz_ros2_control_path + ":" + "/opt/ros/humble/lib" - ) - - resource_path = ( - os.path.dirname(get_package_share_directory("ros2srrc_ur3_gazebo")) - + ":" - + os.path.dirname(get_package_share_directory("ros2srrc_robots")) - + ":" - + os.path.dirname(get_package_share_directory("ros2srrc_endeffectors")) - + ":" - + os.path.dirname(get_package_share_directory("robotiq_description")) - + ":" - + os.path.join( - get_package_share_directory("robotiq_description"), - "world", - "models", - ) - ) - - set_resource_path = SetEnvironmentVariable( - name="GZ_SIM_RESOURCE_PATH", value=resource_path - ) - - set_gz_plugin_path = SetEnvironmentVariable( - name="GZ_SIM_SYSTEM_PLUGIN_PATH", value=gz_plugin_path - ) - - existing_ld = os.environ.get("LD_LIBRARY_PATH", "") - - set_ld_library_path = SetEnvironmentVariable( - name="LD_LIBRARY_PATH", - value=gz_plugin_path + ":/usr/lib/x86_64-linux-gnu:" + existing_ld, - ) - - nodes.append(set_resource_path) - nodes.append(set_gz_plugin_path) - nodes.append(set_ld_library_path) - # ========================= # SRDF (SEMANTIC) # ========================= robot_description_semantic = { "robot_description_semantic": load_file( - "ros2srrc_ur3_moveit2", - "config/ur3robotiq_2f85.srdf" + "ros2srrc_ur3_moveit2", "config/ur3robotiq_2f85.srdf" ) } # ========================= # KINEMATICS # ========================= + kinematics_yaml = load_yaml("ur3_gripper_moveit_config", "config/kinematics.yaml") + kinematics_yaml = { - "robot_description_kinematics": load_yaml( - "ros2srrc_robots", - "ur3/config/kinematics.yaml" - ) + "robot_description_kinematics": kinematics_yaml["/**"]["ros__parameters"] } - # ========================= - # Moveit controller + # CONTROLLERS (MoveIt) # ========================= moveit_controllers = load_yaml( - "ros2srrc_robots", - "ur3/config/moveit_controllers.yaml" + "ur3_gripper_moveit_config", "config/moveit_controllers.yaml" ) - moveit_controllers = moveit_controllers["/**"]["ros__parameters"] - - ompl_planning = load_yaml( - "ros2srrc_robots", - "ur3/config/ompl_planning.yaml" - ) + ompl_planning = load_yaml("ur3_gripper_moveit_config", "config/ompl_planning.yaml") ompl_planning = ompl_planning["/**"]["ros__parameters"] - - joint_limits_yaml = load_yaml( - "ros2srrc_robots", - "ur3/config/joint_limits.yaml" - ) - - pilz_cartesian_limits = load_yaml( - "ros2srrc_robots", - "ur3/config/pilz_cartesian_limits.yaml" - ) - combined_planning = { - "robot_description_planning": - {**joint_limits_yaml, **pilz_cartesian_limits} - } + moveit_controllers = moveit_controllers["/**"]["ros__parameters"] planning_pipelines_config = { - "planning_pipelines": ["ompl"], - "default_planning_pipeline": "ompl", - + "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], + "default_planning_pipeline": "pilz_industrial_motion_planner", "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - - "request_adapters": - "default_planner_request_adapters/AddTimeOptimalParameterization " - "default_planner_request_adapters/ResolveConstraintFrames " - "default_planner_request_adapters/FixWorkspaceBounds " - "default_planner_request_adapters/FixStartStateBounds " - "default_planner_request_adapters/FixStartStateCollision " - "default_planner_request_adapters/FixStartStatePathConstraints", - + }, + "pilz_industrial_motion_planner": { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", + "request_adapters": "", "start_state_max_bounds_error": 0.1, + "default_planner_config": "PTP", }, } move_group_capabilities = { - "capabilities": - "pilz_industrial_motion_planner/MoveGroupSequenceAction " + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction " "pilz_industrial_motion_planner/MoveGroupSequenceService" } + pilz_cartesian_limits = load_yaml( + "ros2srrc_robots", "ur3/config/pilz_cartesian_limits.yaml" + ) + + joint_limits_yaml = load_yaml("ros2srrc_robots", "ur3/config/joint_limits.yaml") + + combined_planning = { + "robot_description_planning": {**joint_limits_yaml, **pilz_cartesian_limits} + } + # ========================= # NODES # ========================= @@ -211,10 +149,8 @@ def launch_setup(context): moveit_controllers, ompl_planning, { - "moveit_controller_manager": - "moveit_simple_controller_manager/MoveItSimpleControllerManager" + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" }, - combined_planning, {"use_sim_time": True}, {"ROB_PARAM": "ur3"}, {"EE_PARAM": "robotiq_2f85"}, @@ -234,8 +170,7 @@ def launch_setup(context): moveit_controllers, ompl_planning, { - "moveit_controller_manager": - "moveit_simple_controller_manager/MoveItSimpleControllerManager" + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" }, {"use_sim_time": True}, {"ROB_PARAM": "ur3"}, @@ -287,7 +222,7 @@ def launch_setup(context): "-topic", "robot_description", "-name", - "ur3", + "ur3_robotiq", "-x", x, "-y", @@ -311,18 +246,10 @@ def launch_setup(context): parameters=[{"use_sim_time": True}], ) - sausage_spawner = Node( - package="custom_robots", - executable="spawn_sausage.py", - name="box_spawner", - output="screen", - ) - nodes.append(robot_state_publisher) nodes.append(static_tf) nodes.append(spawn_robot) nodes.append(clock_bridge) - #nodes.append(sausage_spawner) if sensor == "camera": camera_bridge = Node( @@ -379,6 +306,10 @@ def launch_setup(context): nodes.append(joint_trajectory_controller) nodes.append(gripper_controller) + # ========================= + # MOVEIT + # ========================= + move_group = Node( package="moveit_ros_move_group", executable="move_group", @@ -406,7 +337,7 @@ def launch_setup(context): def generate_launch_description(): declared_arguments = [ - DeclareLaunchArgument("x", default_value="-0.5"), + DeclareLaunchArgument("x", default_value="0"), DeclareLaunchArgument("y", default_value="0"), DeclareLaunchArgument("z", default_value="0.9"), DeclareLaunchArgument("R", default_value="0"), @@ -417,4 +348,4 @@ def generate_launch_description(): return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] - ) \ No newline at end of file + ) diff --git a/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro b/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro new file mode 100644 index 000000000..264fd24a6 --- /dev/null +++ b/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro @@ -0,0 +1,193 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro b/CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro new file mode 100644 index 000000000..18a86c355 --- /dev/null +++ b/CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro @@ -0,0 +1,73 @@ + + + + + + + + transmission_interface/SimpleTransmission + + ${hw_interface} + + + 1 + + + + + transmission_interface/SimpleTransmission + + ${hw_interface} + + + 1 + + + + + transmission_interface/SimpleTransmission + + ${hw_interface} + + + 1 + + + + + transmission_interface/SimpleTransmission + + ${hw_interface} + + + 1 + + + + + transmission_interface/SimpleTransmission + + ${hw_interface} + + + 1 + + + + + transmission_interface/SimpleTransmission + + ${hw_interface} + + + 1 + + + + + + diff --git a/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro b/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro new file mode 100644 index 000000000..359602790 --- /dev/null +++ b/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro @@ -0,0 +1,304 @@ + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + gz_ros2_control/GazeboSimSystem + + + mock_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ur_robot_driver/URPositionHardwareInterface + ${robot_ip} + ${script_filename} + ${output_recipe_filename} + ${input_recipe_filename} + ${headless_mode} + ${reverse_port} + ${script_sender_port} + "${tf_prefix}" + 0 + 2000 + 0.03 + ${use_tool_communication} + ${hash_kinematics} + ${tool_voltage} + ${tool_parity} + ${tool_baud_rate} + ${tool_stop_bits} + ${tool_rx_idle_chars} + ${tool_tx_idle_chars} + ${tool_device_name} + ${tool_tcp_port} + + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + + ${initial_positions['shoulder_pan_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + + ${initial_positions['shoulder_lift_joint']} + + + + + + + {-pi} + {pi} + + + -3.15 + 3.15 + + + + ${initial_positions['elbow_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + ${initial_positions['wrist_1_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + ${initial_positions['wrist_2_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + ${initial_positions['wrist_3_joint']} + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro new file mode 100644 index 000000000..25ac39c8a --- /dev/null +++ b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(arg simulation_controllers) + + + + + + + + + + + $(arg simulation_controllers) + + + + + diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro new file mode 100644 index 000000000..fa9bd19c0 --- /dev/null +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/CustomRobots/robot_arms/models/ur3/ur_generated.urdf b/CustomRobots/robot_arms/models/ur3/ur_generated.urdf new file mode 100644 index 000000000..e69de29bb diff --git a/CustomRobots/robot_arms/models/ur3/ur_macro.xacro b/CustomRobots/robot_arms/models/ur3/ur_macro.xacro new file mode 100644 index 000000000..9d3beab67 --- /dev/null +++ b/CustomRobots/robot_arms/models/ur3/ur_macro.xacro @@ -0,0 +1,284 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Industrial/ur3_gripper_moveit_config /CMakeLists.txt b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt new file mode 100644 index 000000000..f1152fa21 --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(ur5_gripper_moveit_config) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(robotiq_description REQUIRED) +find_package(urdf REQUIRED) +find_package(xacro REQUIRED) + +install(DIRECTORY config rviz srdf + DESTINATION share/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/Industrial/ur3_gripper_moveit_config /config/controllers.yaml b/Industrial/ur3_gripper_moveit_config /config/controllers.yaml new file mode 100644 index 000000000..568d22a69 --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /config/controllers.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller + - gripper_controller + + joint_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + gripper_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: false + joints: + - robotiq_85_left_knuckle_joint diff --git a/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml b/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml new file mode 100644 index 000000000..ff0bdc33f --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + ur3_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + gripper: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin \ No newline at end of file diff --git a/Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml b/Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml new file mode 100644 index 000000000..188c35ddd --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller + - gripper_controller + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + gripper_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - robotiq_85_left_knuckle_joint \ No newline at end of file diff --git a/Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml b/Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml new file mode 100644 index 000000000..1fae3f8d6 --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml @@ -0,0 +1,72 @@ +/**: + ros__parameters: + planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + ur3_arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 diff --git a/Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml b/Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml new file mode 100644 index 000000000..741c97b6a --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml @@ -0,0 +1,76 @@ +############################################### +# 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. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level +# controller performance. It essentially increases the timestep when calculating the target pose, to move the target +# pose farther away. [seconds] +system_latency_compensation: 0.04 + +## Properties of outgoing commands +publish_period: 0.004 # 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: std_msgs/Float64MultiArray + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +## Incoming Joint State properties +low_pass_filter_coeff: 10.0 # Larger --> trust the filtered data more, trust the measurements less. + +## MoveIt properties +move_group_name: ur3_arm # Often 'manipulator' or 'arm' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: tool0 # 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: 100.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 200.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. + +## 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: /forward_position_controller/commands # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: threshold_distance +# Parameters for "threshold_distance"-type collision checking +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] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/Industrial/ur3_gripper_moveit_config /package.xml b/Industrial/ur3_gripper_moveit_config /package.xml new file mode 100644 index 000000000..a2fb72a71 --- /dev/null +++ b/Industrial/ur3_gripper_moveit_config /package.xml @@ -0,0 +1,32 @@ + + + + ur5_gripper_moveit_config + 0.0.0 + TODO: Package description + justin + TODO: License declaration + + ament_cmake + ament_cmake_python + + robotiq_description + urdf + xacro + + launch + launch_ros + moveit_kinematics + moveit_planners_ompl + moveit_ros_move_group + moveit_ros_visualization + moveit_servo + moveit_simple_controller_manager + controller_manager + joint_trajectory_controller + rviz2 + + + ament_cmake + + From f22e3b42d66cb927bb4253bd3a4c3afc52134516 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 17:38:03 +0200 Subject: [PATCH 39/88] Fix cmakelists and package.xml of ur3_gripper_moveit_config --- Industrial/ur3_gripper_moveit_config /CMakeLists.txt | 2 +- Industrial/ur3_gripper_moveit_config /package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Industrial/ur3_gripper_moveit_config /CMakeLists.txt b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt index f1152fa21..2cd99584e 100644 --- a/Industrial/ur3_gripper_moveit_config /CMakeLists.txt +++ b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(ur5_gripper_moveit_config) +project(ur3_gripper_moveit_config) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/Industrial/ur3_gripper_moveit_config /package.xml b/Industrial/ur3_gripper_moveit_config /package.xml index a2fb72a71..b5e616ab1 100644 --- a/Industrial/ur3_gripper_moveit_config /package.xml +++ b/Industrial/ur3_gripper_moveit_config /package.xml @@ -1,7 +1,7 @@ - ur5_gripper_moveit_config + ur3_gripper_moveit_config 0.0.0 TODO: Package description justin From 226e03c7f803af6f2e6f84d7bc64cc1802997f79 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 17:48:52 +0200 Subject: [PATCH 40/88] Fix cmakelist of ur3_gripper_moveit_config --- Industrial/ur3_gripper_moveit_config /CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Industrial/ur3_gripper_moveit_config /CMakeLists.txt b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt index 2cd99584e..e2513fb58 100644 --- a/Industrial/ur3_gripper_moveit_config /CMakeLists.txt +++ b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt @@ -12,7 +12,7 @@ find_package(robotiq_description REQUIRED) find_package(urdf REQUIRED) find_package(xacro REQUIRED) -install(DIRECTORY config rviz srdf +install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) From 3147a1247cdf00acea45a58a102b40b00de3e5c1 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 19:07:05 +0200 Subject: [PATCH 41/88] Change ur3.urdf.xacro --- .../robot_arms/models/ur3/ur3.urdf.xacro | 69 ++++++++++++++----- 1 file changed, 53 insertions(+), 16 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index fa9bd19c0..ad94af596 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -1,7 +1,5 @@ - - - + + @@ -11,28 +9,49 @@ + + + + - - - - - + + + + + + + + + + + + + + + + + + - + + @@ -41,20 +60,38 @@ - + - - + + + + - + - - + From e0914e52d820b6b469a3770bd8ce906ecbf3f7ba Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 19:33:50 +0200 Subject: [PATCH 42/88] Change ur.urdf,xacro --- .../robot_arms/models/ur3/ur.urdf.xacro | 112 ++++++++++++------ 1 file changed, 73 insertions(+), 39 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro index 25ac39c8a..90096f189 100644 --- a/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro @@ -1,83 +1,117 @@ + + - - + - - - - + + + + + + + + - - + + + + - - - - - - - - - - + + + + + + + + + + + - - - - - + + + + + + + - - + + - - + + - - + + - - + + + + + - - + - - $(arg simulation_controllers) + + + + $(arg simulation_controllers) + + + - + - - $(arg simulation_controllers) + + + + $(arg simulation_controllers) + + - + \ No newline at end of file From 1bc8cccc2e957be6ece8835dfd52779c3120d7fe Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 20:50:07 +0200 Subject: [PATCH 43/88] Change stack --- .../robot_arms/models/ur3/inc/ur_common.xacro | 15 ++ .../models/ur3/ur.ros2_control.xacro | 50 ++-- .../robot_arms/models/ur3/ur.urdf.xacro | 48 +++- .../robot_arms/models/ur3/ur3.urdf.xacro | 48 +--- .../robot_arms/models/ur3/ur_macro.xacro | 230 +++++++++++------- .../config/ur3robotiq_2f85.srdf | 20 +- 6 files changed, 240 insertions(+), 171 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro b/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro index 264fd24a6..4281d0033 100644 --- a/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro +++ b/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro @@ -187,6 +187,21 @@ + + + + + + diff --git a/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro b/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro index 359602790..1387428c5 100644 --- a/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro @@ -58,12 +58,12 @@ - {-2*pi} - {2*pi} + {-2*pi} + {2*pi} - -3.15 - 3.15 + -3.15 + 3.15 @@ -74,12 +74,12 @@ - {-2*pi} - {2*pi} + {-2*pi} + {2*pi} - -3.15 - 3.15 + -3.15 + 3.15 @@ -90,12 +90,12 @@ - {-pi} - {pi} + {-pi} + {pi} - -3.15 - 3.15 + -3.15 + 3.15 @@ -106,12 +106,12 @@ - {-2*pi} - {2*pi} + {-2*pi} + {2*pi} - -3.2 - 3.2 + -3.2 + 3.2 @@ -122,12 +122,12 @@ - {-2*pi} - {2*pi} + {-2*pi} + {2*pi} - -3.2 - 3.2 + -3.2 + 3.2 @@ -138,12 +138,12 @@ - {-2*pi} - {2*pi} + {-2*pi} + {2*pi} - -3.2 - 3.2 + -3.2 + 3.2 @@ -301,4 +301,4 @@ - + \ No newline at end of file diff --git a/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro index 90096f189..d940074dc 100644 --- a/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro @@ -13,10 +13,17 @@ - - - - + + + + + + + @@ -70,13 +77,44 @@ prefix="$(arg prefix)" parent="world" + joint_limits_parameters_file="$(arg joint_limit_params)" + kinematics_parameters_file="$(arg kinematics_params)" + physical_parameters_file="$(arg physical_params)" + visual_parameters_file="$(arg visual_params)" + + transmission_hw_interface="$(arg transmission_hw_interface)" + + safety_limits="$(arg safety_limits)" + safety_pos_margin="$(arg safety_pos_margin)" + safety_k_position="$(arg safety_k_position)" + + use_fake_hardware="$(arg use_fake_hardware)" + fake_sensor_commands="$(arg fake_sensor_commands)" + + sim_gazebo="$(arg sim_gazebo)" + sim_gz="$(arg sim_gz)" + + headless_mode="$(arg headless_mode)" + + initial_positions="${load_yaml(initial_positions_file)}" + + use_tool_communication="$(arg use_tool_communication)" + tool_voltage="$(arg tool_voltage)" + tool_parity="$(arg tool_parity)" + tool_baud_rate="$(arg tool_baud_rate)" + tool_stop_bits="$(arg tool_stop_bits)" + tool_rx_idle_chars="$(arg tool_rx_idle_chars)" + tool_tx_idle_chars="$(arg tool_tx_idle_chars)" + tool_device_name="$(arg tool_device_name)" + tool_tcp_port="$(arg tool_tcp_port)" + robot_ip="$(arg robot_ip)" script_filename="$(arg script_filename)" output_recipe_filename="$(arg output_recipe_filename)" input_recipe_filename="$(arg input_recipe_filename)"> - + diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index ad94af596..08973e1ad 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -35,56 +35,20 @@ + - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - + + + + + + + visual_parameters_file + + transmission_hw_interface:=hardware_interface/PositionJointInterface + + safety_limits:=false + safety_pos_margin:=0.15 + safety_k_position:=20 + + use_fake_hardware:=false + fake_sensor_commands:=false + + sim_gazebo:=false + sim_gz:=false + + headless_mode:=false + + initial_positions:=${dict( + shoulder_pan_joint=0.0, + shoulder_lift_joint=-1.57, + elbow_joint=0.0, + wrist_1_joint=-1.57, + wrist_2_joint=0.0, + wrist_3_joint=0.0)} + + use_tool_communication:=false + + tool_voltage:=24 + tool_parity:=0 + tool_baud_rate:=115200 + tool_stop_bits:=1 + tool_rx_idle_chars:=1.5 + tool_tx_idle_chars:=3.5 + + tool_device_name:=/tmp/ttyUR + tool_tcp_port:=54321 + + robot_ip:=0.0.0.0 + + script_filename:=to_be_filled_by_ur_robot_driver + output_recipe_filename:=to_be_filled_by_ur_robot_driver + input_recipe_filename:=to_be_filled_by_ur_robot_driver - - + reverse_port:=50001 + script_sender_port:=50002"> + + + - + - - - + - + - + - + - + @@ -50,13 +125,13 @@ - + - + - + @@ -67,13 +142,13 @@ - + - + - + @@ -84,82 +159,78 @@ - + - + - + - + - - + - + - + - + - + - - + - + - + - + - + - - + - + - + - + - + - + - - - + + + - @@ -175,73 +246,61 @@ - - - - - + + - - - - - + + - - - - - + + - - - - - + + - - - - - + + - - - - - + + - - - + + + @@ -276,9 +335,10 @@ - - - + + + + - + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf index b7e7dd526..f4455b9bb 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf @@ -39,20 +39,12 @@ - - - - - - - - - - - - - - + + + + + + From 3da41f30615fc99c9a9e773b9f98f4efa5ea648e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 21:39:05 +0200 Subject: [PATCH 44/88] add gripper control to xacro --- .../robot_arms/models/ur3/ur3.urdf.xacro | 8 + .../urdf/robotiq_2f85_ros2control.xacro | 138 +++++++++++------- 2 files changed, 93 insertions(+), 53 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 08973e1ad..6547e3263 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -40,6 +40,8 @@ + + @@ -58,4 +60,10 @@ tf_prefix="" hash_kinematics="" robot_ip="$(arg robot_ip)"/> + + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro index f21a7a0ac..e6acb4730 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro @@ -1,57 +1,89 @@ - - - - - - - 0.0 - 0.80285 - - - 0.0 - - - - - - - - robotiq_85_left_knuckle_joint - 1.0 - - - - - - - robotiq_85_left_knuckle_joint - 1.0 - - - - - - robotiq_85_left_knuckle_joint - 1.0 - - - - - - robotiq_85_left_knuckle_joint - -1.0 - - - - - - robotiq_85_left_knuckle_joint - -1.0 - - - + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + mock_components/GenericSystem + + + + + + + + + + 0.0 + 0.80285 + + + + 0.0 + + + + + + + + + + + robotiq_85_left_knuckle_joint + 1.0 + + + + + + + + robotiq_85_left_knuckle_joint + 1.0 + + + + + + + robotiq_85_left_knuckle_joint + 1.0 + + + + + + + robotiq_85_left_knuckle_joint + -1.0 + + + + + + + robotiq_85_left_knuckle_joint + -1.0 + + + + + + + - \ No newline at end of file + + \ No newline at end of file From 39a7eb81a6c7d8a22b82eb15a24dc43f10f04cf1 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 22:00:53 +0200 Subject: [PATCH 45/88] add gripper control to xacro --- CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 6547e3263..d9e70d550 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -40,7 +40,7 @@ - + - + From 92731aeec21267cf6f9ea4e1e7fe8707cef8fbbd Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 23:13:36 +0200 Subject: [PATCH 46/88] add gripper control to xacro --- CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro | 3 --- 1 file changed, 3 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index d9e70d550..fa79ba759 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -36,9 +36,6 @@ - - - From e9465f7d8a2df656af5c4ee77bfb5e9f0572bb47 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Thu, 25 Jun 2026 23:34:47 +0200 Subject: [PATCH 47/88] Fix rviz launcher --- CustomRobots/robot_arms/launch/ur3.launch.py | 339 ++++-------------- .../robot_arms/models/ur3/ur3.urdf.xacro | 2 - Launchers/rviz/sausage_exercise.launch.py | 139 +++---- 3 files changed, 122 insertions(+), 358 deletions(-) diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index 7c5aa7cd3..a2391138c 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -1,18 +1,17 @@ -from launch.actions import ( - IncludeLaunchDescription, - DeclareLaunchArgument, - SetEnvironmentVariable, -) -from launch.substitutions import LaunchConfiguration -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch import LaunchDescription -from launch_ros.actions import Node +""" +UR3 + Robotiq - RViz Launcher +Launches ONLY RViz. +Assumes Gazebo, controllers and move_group are already running. +""" import os -import xacro import yaml +import xacro + +from launch import LaunchDescription +from launch.actions import TimerAction +from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -from launch.actions import DeclareLaunchArgument, OpaqueFunction def load_file(package_name, file_path): @@ -27,24 +26,14 @@ def load_yaml(package_name, file_path): return yaml.safe_load(f) -def launch_setup(context): - x = LaunchConfiguration("x") - y = LaunchConfiguration("y") - z = LaunchConfiguration("z") - R = LaunchConfiguration("R") - P = LaunchConfiguration("P") - Y = LaunchConfiguration("Y") - gz_sensor = LaunchConfiguration("sensor") +def generate_launch_description(): package_dir = get_package_share_directory("custom_robots") - sensor = gz_sensor.perform(context) - - nodes = [] + # -------------------------------------------------- + # Robot Description + # -------------------------------------------------- - # ========================= - # ROBOT DESCRIPTION (URDF) - # ========================= xacro_file = os.path.join( package_dir, "models", @@ -52,7 +41,11 @@ def launch_setup(context): "ur3.urdf.xacro", ) - controllers_file = os.path.join(package_dir, "config", "ur3_controllers.yaml") + controllers_file = os.path.join( + package_dir, + "config", + "ur3_controllers.yaml", + ) robot_description_content = xacro.process_file( xacro_file, @@ -67,285 +60,85 @@ def launch_setup(context): "hmi": "false", "EE": "true", "EE_name": "robotiq_2f85", - "camera": "true" if sensor == "camera" else "false", + "camera": "false", }, ).toxml() - robot_description = {"robot_description": robot_description_content} + robot_description = { + "robot_description": robot_description_content + } + + # -------------------------------------------------- + # Semantic Description + # -------------------------------------------------- - # ========================= - # SRDF (SEMANTIC) - # ========================= robot_description_semantic = { "robot_description_semantic": load_file( - "ros2srrc_ur3_moveit2", "config/ur3robotiq_2f85.srdf" + "ros2srrc_ur3_moveit2", + "config/ur3robotiq_2f85.srdf", ) } - # ========================= - # KINEMATICS - # ========================= - kinematics_yaml = load_yaml("ur3_gripper_moveit_config", "config/kinematics.yaml") - - kinematics_yaml = { - "robot_description_kinematics": kinematics_yaml["/**"]["ros__parameters"] - } + # -------------------------------------------------- + # Kinematics + # -------------------------------------------------- - # ========================= - # CONTROLLERS (MoveIt) - # ========================= - moveit_controllers = load_yaml( - "ur3_gripper_moveit_config", "config/moveit_controllers.yaml" + kinematics_yaml = load_yaml( + "ur3_gripper_moveit_config", + "config/kinematics.yaml", ) - ompl_planning = load_yaml("ur3_gripper_moveit_config", "config/ompl_planning.yaml") - - ompl_planning = ompl_planning["/**"]["ros__parameters"] - - moveit_controllers = moveit_controllers["/**"]["ros__parameters"] - - planning_pipelines_config = { - "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], - "default_planning_pipeline": "pilz_industrial_motion_planner", - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - }, - "pilz_industrial_motion_planner": { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - "request_adapters": "", - "start_state_max_bounds_error": 0.1, - "default_planner_config": "PTP", - }, + kinematics_yaml = { + "robot_description_kinematics": + kinematics_yaml["/**"]["ros__parameters"] } - move_group_capabilities = { - "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction " - "pilz_industrial_motion_planner/MoveGroupSequenceService" - } + # -------------------------------------------------- + # OMPL Planning + # -------------------------------------------------- - pilz_cartesian_limits = load_yaml( - "ros2srrc_robots", "ur3/config/pilz_cartesian_limits.yaml" + ompl_planning = load_yaml( + "ur3_gripper_moveit_config", + "config/ompl_planning.yaml", ) - joint_limits_yaml = load_yaml("ros2srrc_robots", "ur3/config/joint_limits.yaml") - - combined_planning = { - "robot_description_planning": {**joint_limits_yaml, **pilz_cartesian_limits} - } + ompl_planning = ompl_planning["/**"]["ros__parameters"] - # ========================= - # NODES - # ========================= + # -------------------------------------------------- + # RViz configuration + # -------------------------------------------------- - move = Node( - package="ros2srrc_execution", - executable="move", - name="move_action_server", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - moveit_controllers, - ompl_planning, - { - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" - }, - {"use_sim_time": True}, - {"ROB_PARAM": "ur3"}, - {"EE_PARAM": "robotiq_2f85"}, - {"ROB_GROUP": "ur3_arm"}, - ], + rviz_config_file = os.path.join( + get_package_share_directory("ros2srrc_ur3_moveit2"), + "rviz", + "moveit.rviz", ) - robmove = Node( - package="ros2srrc_execution", - executable="robmove", - name="Robmove", + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - moveit_controllers, - ompl_planning, - { - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" - }, - {"use_sim_time": True}, - {"ROB_PARAM": "ur3"}, - {"ROB_GROUP": "ur3_arm"}, + arguments=[ + "-d", + rviz_config_file, ], - ) - - robpose = Node( - package="ros2srrc_execution", - executable="robpose", - name="robpose", - output="screen", parameters=[ robot_description, robot_description_semantic, kinematics_yaml, ompl_planning, {"use_sim_time": True}, - {"ROB_PARAM": "ur3"}, - {"ROB_GROUP": "ur3_arm"}, ], ) - nodes.append(move) - nodes.append(robmove) - nodes.append(robpose) - - # ========================= - # CORE NODES - # ========================= - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - parameters=[robot_description, {"use_sim_time": True}], - output="screen", - ) - - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - arguments=[x, y, z, R, P, Y, "world", "base_link"], - ) - - spawn_robot = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-topic", - "robot_description", - "-name", - "ur3_robotiq", - "-x", - x, - "-y", - y, - "-z", - z, - "-R", - R, - "-P", - P, - "-Y", - Y, - ], - output="screen", - ) - - clock_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock]"], - parameters=[{"use_sim_time": True}], - ) - - nodes.append(robot_state_publisher) - nodes.append(static_tf) - nodes.append(spawn_robot) - nodes.append(clock_bridge) - - if sensor == "camera": - camera_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=[ - "/hand_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", - "/base_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", - ], - output="screen", - ) - - gz_ros2_image_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - arguments=["/hand_camera/image"], - output="screen", - ) - - gz_ros2_base_image_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - arguments=["/base_camera/image"], - output="screen", - ) - - nodes.append(camera_bridge) - nodes.append(gz_ros2_image_bridge) - nodes.append(gz_ros2_base_image_bridge) - - # ========================= - # CONTROLLERS - # ========================= - - joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], + delay_rviz = TimerAction( + period=3.0, + actions=[rviz_node], ) - joint_trajectory_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_trajectory_controller"], - ) - - gripper_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["gripper_controller"], - ) - - nodes.append(joint_state_broadcaster) - nodes.append(joint_trajectory_controller) - nodes.append(gripper_controller) - - # ========================= - # MOVEIT - # ========================= - - move_group = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - planning_pipelines_config, - move_group_capabilities, - moveit_controllers, - combined_planning, - {"use_sim_time": True}, - ], - ) - - nodes.append(move_group) - - # ========================= - # LAUNCH - # ========================= - - return nodes - - -def generate_launch_description(): - declared_arguments = [ - DeclareLaunchArgument("x", default_value="0"), - DeclareLaunchArgument("y", default_value="0"), - DeclareLaunchArgument("z", default_value="0.9"), - DeclareLaunchArgument("R", default_value="0"), - DeclareLaunchArgument("P", default_value="0"), - DeclareLaunchArgument("Y", default_value="0"), - DeclareLaunchArgument("sensor", default_value="none"), - ] - return LaunchDescription( - declared_arguments + [OpaqueFunction(function=launch_setup)] - ) + [ + delay_rviz, + ] + ) \ No newline at end of file diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index fa79ba759..939c61b31 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -43,8 +43,6 @@ bringup="false" parent_link="tool0"/> - - Date: Thu, 25 Jun 2026 23:53:42 +0200 Subject: [PATCH 48/88] Fix rviz launcher --- CustomRobots/robot_arms/launch/ur3.launch.py | 339 +++++++++++++++---- 1 file changed, 273 insertions(+), 66 deletions(-) diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py index a2391138c..7c5aa7cd3 100644 --- a/CustomRobots/robot_arms/launch/ur3.launch.py +++ b/CustomRobots/robot_arms/launch/ur3.launch.py @@ -1,17 +1,18 @@ -""" -UR3 + Robotiq - RViz Launcher -Launches ONLY RViz. -Assumes Gazebo, controllers and move_group are already running. -""" +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, + SetEnvironmentVariable, +) +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch import LaunchDescription +from launch_ros.actions import Node import os -import yaml import xacro - -from launch import LaunchDescription -from launch.actions import TimerAction -from launch_ros.actions import Node +import yaml from ament_index_python.packages import get_package_share_directory +from launch.actions import DeclareLaunchArgument, OpaqueFunction def load_file(package_name, file_path): @@ -26,14 +27,24 @@ def load_yaml(package_name, file_path): return yaml.safe_load(f) -def generate_launch_description(): +def launch_setup(context): + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + R = LaunchConfiguration("R") + P = LaunchConfiguration("P") + Y = LaunchConfiguration("Y") + gz_sensor = LaunchConfiguration("sensor") package_dir = get_package_share_directory("custom_robots") - # -------------------------------------------------- - # Robot Description - # -------------------------------------------------- + sensor = gz_sensor.perform(context) + + nodes = [] + # ========================= + # ROBOT DESCRIPTION (URDF) + # ========================= xacro_file = os.path.join( package_dir, "models", @@ -41,11 +52,7 @@ def generate_launch_description(): "ur3.urdf.xacro", ) - controllers_file = os.path.join( - package_dir, - "config", - "ur3_controllers.yaml", - ) + controllers_file = os.path.join(package_dir, "config", "ur3_controllers.yaml") robot_description_content = xacro.process_file( xacro_file, @@ -60,85 +67,285 @@ def generate_launch_description(): "hmi": "false", "EE": "true", "EE_name": "robotiq_2f85", - "camera": "false", + "camera": "true" if sensor == "camera" else "false", }, ).toxml() - robot_description = { - "robot_description": robot_description_content - } - - # -------------------------------------------------- - # Semantic Description - # -------------------------------------------------- + robot_description = {"robot_description": robot_description_content} + # ========================= + # SRDF (SEMANTIC) + # ========================= robot_description_semantic = { "robot_description_semantic": load_file( - "ros2srrc_ur3_moveit2", - "config/ur3robotiq_2f85.srdf", + "ros2srrc_ur3_moveit2", "config/ur3robotiq_2f85.srdf" ) } - # -------------------------------------------------- - # Kinematics - # -------------------------------------------------- + # ========================= + # KINEMATICS + # ========================= + kinematics_yaml = load_yaml("ur3_gripper_moveit_config", "config/kinematics.yaml") + + kinematics_yaml = { + "robot_description_kinematics": kinematics_yaml["/**"]["ros__parameters"] + } - kinematics_yaml = load_yaml( - "ur3_gripper_moveit_config", - "config/kinematics.yaml", + # ========================= + # CONTROLLERS (MoveIt) + # ========================= + moveit_controllers = load_yaml( + "ur3_gripper_moveit_config", "config/moveit_controllers.yaml" ) - kinematics_yaml = { - "robot_description_kinematics": - kinematics_yaml["/**"]["ros__parameters"] + ompl_planning = load_yaml("ur3_gripper_moveit_config", "config/ompl_planning.yaml") + + ompl_planning = ompl_planning["/**"]["ros__parameters"] + + moveit_controllers = moveit_controllers["/**"]["ros__parameters"] + + planning_pipelines_config = { + "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], + "default_planning_pipeline": "pilz_industrial_motion_planner", + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + }, + "pilz_industrial_motion_planner": { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", + "request_adapters": "", + "start_state_max_bounds_error": 0.1, + "default_planner_config": "PTP", + }, } - # -------------------------------------------------- - # OMPL Planning - # -------------------------------------------------- + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction " + "pilz_industrial_motion_planner/MoveGroupSequenceService" + } - ompl_planning = load_yaml( - "ur3_gripper_moveit_config", - "config/ompl_planning.yaml", + pilz_cartesian_limits = load_yaml( + "ros2srrc_robots", "ur3/config/pilz_cartesian_limits.yaml" ) - ompl_planning = ompl_planning["/**"]["ros__parameters"] + joint_limits_yaml = load_yaml("ros2srrc_robots", "ur3/config/joint_limits.yaml") + + combined_planning = { + "robot_description_planning": {**joint_limits_yaml, **pilz_cartesian_limits} + } - # -------------------------------------------------- - # RViz configuration - # -------------------------------------------------- + # ========================= + # NODES + # ========================= - rviz_config_file = os.path.join( - get_package_share_directory("ros2srrc_ur3_moveit2"), - "rviz", - "moveit.rviz", + move = Node( + package="ros2srrc_execution", + executable="move", + name="move_action_server", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + moveit_controllers, + ompl_planning, + { + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" + }, + {"use_sim_time": True}, + {"ROB_PARAM": "ur3"}, + {"EE_PARAM": "robotiq_2f85"}, + {"ROB_GROUP": "ur3_arm"}, + ], ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", + robmove = Node( + package="ros2srrc_execution", + executable="robmove", + name="Robmove", output="screen", - arguments=[ - "-d", - rviz_config_file, + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + moveit_controllers, + ompl_planning, + { + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" + }, + {"use_sim_time": True}, + {"ROB_PARAM": "ur3"}, + {"ROB_GROUP": "ur3_arm"}, ], + ) + + robpose = Node( + package="ros2srrc_execution", + executable="robpose", + name="robpose", + output="screen", parameters=[ robot_description, robot_description_semantic, kinematics_yaml, ompl_planning, {"use_sim_time": True}, + {"ROB_PARAM": "ur3"}, + {"ROB_GROUP": "ur3_arm"}, ], ) - delay_rviz = TimerAction( - period=3.0, - actions=[rviz_node], + nodes.append(move) + nodes.append(robmove) + nodes.append(robpose) + + # ========================= + # CORE NODES + # ========================= + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[robot_description, {"use_sim_time": True}], + output="screen", + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=[x, y, z, R, P, Y, "world", "base_link"], + ) + + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", + "robot_description", + "-name", + "ur3_robotiq", + "-x", + x, + "-y", + y, + "-z", + z, + "-R", + R, + "-P", + P, + "-Y", + Y, + ], + output="screen", + ) + + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock]"], + parameters=[{"use_sim_time": True}], + ) + + nodes.append(robot_state_publisher) + nodes.append(static_tf) + nodes.append(spawn_robot) + nodes.append(clock_bridge) + + if sensor == "camera": + camera_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "/hand_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + "/base_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + ], + output="screen", + ) + + gz_ros2_image_bridge = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/hand_camera/image"], + output="screen", + ) + + gz_ros2_base_image_bridge = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/base_camera/image"], + output="screen", + ) + + nodes.append(camera_bridge) + nodes.append(gz_ros2_image_bridge) + nodes.append(gz_ros2_base_image_bridge) + + # ========================= + # CONTROLLERS + # ========================= + + joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], ) + joint_trajectory_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller"], + ) + + gripper_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["gripper_controller"], + ) + + nodes.append(joint_state_broadcaster) + nodes.append(joint_trajectory_controller) + nodes.append(gripper_controller) + + # ========================= + # MOVEIT + # ========================= + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + planning_pipelines_config, + move_group_capabilities, + moveit_controllers, + combined_planning, + {"use_sim_time": True}, + ], + ) + + nodes.append(move_group) + + # ========================= + # LAUNCH + # ========================= + + return nodes + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument("x", default_value="0"), + DeclareLaunchArgument("y", default_value="0"), + DeclareLaunchArgument("z", default_value="0.9"), + DeclareLaunchArgument("R", default_value="0"), + DeclareLaunchArgument("P", default_value="0"), + DeclareLaunchArgument("Y", default_value="0"), + DeclareLaunchArgument("sensor", default_value="none"), + ] + return LaunchDescription( - [ - delay_rviz, - ] - ) \ No newline at end of file + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) From 368bc9839e4bacababe6384d64a41271c0c69ef5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 00:06:26 +0200 Subject: [PATCH 49/88] Fix rviz launcher --- Launchers/rviz/sausage_exercise.launch.py | 139 +++++++++++++--------- 1 file changed, 83 insertions(+), 56 deletions(-) diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py index ec0b9ebc4..3c3af64b3 100644 --- a/Launchers/rviz/sausage_exercise.launch.py +++ b/Launchers/rviz/sausage_exercise.launch.py @@ -1,85 +1,113 @@ """ -Pick Place Harmonic - RViz + MoveIt Launcher -Launches ONLY: MoveIt move_group + RViz with motion planning +UR3 + Robotiq - RViz Launcher +Launches ONLY RViz with MoveIt Motion Planning Assumes Gazebo and robot are already running """ import os +import xacro +import yaml + from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import TimerAction + from ament_index_python.packages import get_package_share_directory -import xacro + +def load_yaml(package_name, file_path): + pkg_path = get_package_share_directory(package_name) + with open(os.path.join(pkg_path, file_path), "r") as f: + return yaml.safe_load(f) def generate_launch_description(): - # Get package directories - pkg_share_dir = get_package_share_directory("custom_robots") - moveit_config_package = "ur3_gripper_moveit_config" - # Robot description (must match what's in Gazebo) - xacro_file = os.path.join(pkg_share_dir, "models/ur3", "ur3.urdf.xacro") - controllers_file = os.path.join(pkg_share_dir, "config", "ur3_controllers.yaml") + # ===================================================== + # Packages + # ===================================================== + + moveit_pkg_share = get_package_share_directory( + "ros2srrc_ur3_moveit2" + ) + + + gazebo_pkg_share = get_package_share_directory( + "ros2srrc_ur3_gazebo" + ) + + # ===================================================== + # URDF + # ===================================================== + + xacro_file = os.path.join( + gazebo_pkg_share, + "urdf", + "ur3_robotiq_2f85.urdf.xacro", + ) robot_description_content = xacro.process_file( xacro_file, mappings={ - "ur_type": "ur3", - "name": "ur", - "prefix": "", - "use_fake_hardware": "false", - "sim_gazebo": "false", - "sim_gz": "true", - "simulation_controllers": controllers_file, + "bringup": "false", + "hmi": "false", + "robot_ip": "0.0.0.0", + "EE": "true", + "EE_name": "robotiq_2f85", + "camera": "false", + "script_filename": "none", + "input_recipe_filename": "none", + "output_recipe_filename": "none", }, ).toxml() - robot_description = {"robot_description": robot_description_content} + robot_description = { + "robot_description": robot_description_content + } + # ===================================================== # SRDF - srdf_file = os.path.join("ros2srrc_ur3_moveit2", "config", "ur5_robotiq.srdf") - with open(srdf_file, "r") as file: - robot_description_semantic = {"robot_description_semantic": file.read()} - - # Kinematics - kinematics_yaml = os.path.join(moveit_pkg_share, "config", "kinematics.yaml") - - # OMPL planning - ompl_planning_yaml = os.path.join(moveit_pkg_share, "config", "ompl_planning.yaml") + # ===================================================== - # MoveIt controllers - moveit_controllers = os.path.join(moveit_pkg_share, "config", "controllers.yaml") + srdf_file = os.path.join( + moveit_pkg_share, + "config", + "ur3robotiq_2f85.srdf", + ) - # Planning pipeline - planning_pipelines_config = { - "planning_pipelines": ["ompl"], - "default_planning_pipeline": "ompl", - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", - "start_state_max_bounds_error": 0.1, - }, + with open(srdf_file, "r") as file: + robot_description_semantic = { + "robot_description_semantic": file.read() + } + + # ===================================================== + # MoveIt Config Files + # ===================================================== + + kinematics_yaml = { + "robot_description_kinematics": load_yaml( + "ros2srrc_robots", + "ur3/config/kinematics.yaml", + ) } - # Trajectory execution - trajectory_execution = { - "moveit_manage_controllers": True, - "trajectory_execution.allowed_execution_duration_scaling": 1.2, - "trajectory_execution.allowed_goal_duration_margin": 0.5, - "trajectory_execution.allowed_start_tolerance": 0.01, - } + ompl_planning = load_yaml( + "ros2srrc_robots", + "ur3/config/ompl_planning.yaml", + ) - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } + ompl_planning = ompl_planning["/**"]["ros__parameters"] - # MoveIt move_group node + # ===================================================== + # RViz Config + # ===================================================== - # RViz with MoveIt configuration - rviz_config_file = os.path.join(moveit_pkg_share, "rviz", "moveit.rviz") + rviz_config_file = ( + "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/" + "packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz" + ) + + print("MOVEIT PKG =", moveit_pkg_share) + print("RVIZ FILE =", rviz_config_file) rviz_node = Node( package="rviz2", @@ -90,13 +118,12 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, - ompl_planning_yaml, + ompl_planning, kinematics_yaml, {"use_sim_time": True}, ], ) - # Delay RViz after MoveIt delay_rviz = TimerAction( period=3.0, actions=[rviz_node], @@ -106,4 +133,4 @@ def generate_launch_description(): [ delay_rviz, ] - ) + ) \ No newline at end of file From 2c24b1818bf75083c612ef891b38f57331734a72 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 00:18:28 +0200 Subject: [PATCH 50/88] Fix rviz launcher --- Launchers/rviz/sausage_exercise.launch.py | 46 +++++++++++++---------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py index 3c3af64b3..aeccab830 100644 --- a/Launchers/rviz/sausage_exercise.launch.py +++ b/Launchers/rviz/sausage_exercise.launch.py @@ -26,37 +26,40 @@ def generate_launch_description(): # Packages # ===================================================== - moveit_pkg_share = get_package_share_directory( - "ros2srrc_ur3_moveit2" - ) - - - gazebo_pkg_share = get_package_share_directory( - "ros2srrc_ur3_gazebo" - ) + pkg_share_dir = get_package_share_directory("custom_robots") + moveit_pkg_share = get_package_share_directory("ros2srrc_ur3_moveit2") # ===================================================== # URDF # ===================================================== xacro_file = os.path.join( - gazebo_pkg_share, - "urdf", - "ur3_robotiq_2f85.urdf.xacro", + pkg_share_dir, + "models", + "ur3", + "ur3.urdf.xacro", + ) + + controllers_file = os.path.join( + pkg_share_dir, + "config", + "ur3_controllers.yaml", ) robot_description_content = xacro.process_file( xacro_file, mappings={ - "bringup": "false", + "ur_type": "ur3", + "name": "ur", + "prefix": "", + "use_fake_hardware": "false", + "sim_gazebo": "false", + "sim_gz": "true", + "simulation_controllers": controllers_file, "hmi": "false", - "robot_ip": "0.0.0.0", "EE": "true", "EE_name": "robotiq_2f85", "camera": "false", - "script_filename": "none", - "input_recipe_filename": "none", - "output_recipe_filename": "none", }, ).toxml() @@ -83,11 +86,14 @@ def generate_launch_description(): # MoveIt Config Files # ===================================================== + kinematics_yaml = load_yaml( + "ur3_gripper_moveit_config", + "config/kinematics.yaml", + ) + kinematics_yaml = { - "robot_description_kinematics": load_yaml( - "ros2srrc_robots", - "ur3/config/kinematics.yaml", - ) + "robot_description_kinematics": + kinematics_yaml["/**"]["ros__parameters"] } ompl_planning = load_yaml( From 384cedeed77f7dcc9fe4d33581210f5ebf89b5b2 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 00:51:35 +0200 Subject: [PATCH 51/88] Fix macro and ros2control for gripper --- .../robot_arms/models/ur3/ur3.urdf.xacro | 4 +- .../urdf/robotiq_2f85_macro.urdf.xacro | 92 ++++++++++--------- .../urdf/robotiq_2f85_ros2control.xacro | 56 +++++++---- 3 files changed, 87 insertions(+), 65 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 939c61b31..9964bd5ee 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -41,7 +41,8 @@ + parent_link="tool0" + prefix=""/> diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro index 55aee203a..d05860bfc 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro @@ -35,7 +35,11 @@ - + @@ -80,7 +84,7 @@ - + @@ -100,14 +104,14 @@ - + - + - + @@ -129,18 +133,18 @@ - - - + + + - + - + @@ -161,18 +165,18 @@ - - - + + + - + - + - + @@ -192,15 +196,15 @@ - - - + + + - + @@ -220,15 +224,15 @@ - - - + + + - + @@ -248,18 +252,18 @@ - - - + + + - + - + @@ -279,18 +283,18 @@ - - - + + + - + - + @@ -312,18 +316,18 @@ - - - + + + - + - + @@ -345,14 +349,14 @@ - - - + + + - + @@ -377,7 +381,7 @@ - + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro index e6acb4730..94c18e692 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro @@ -2,10 +2,11 @@ + params=" + name + prefix + sim_gz:=true + use_fake_hardware:=false"> @@ -23,7 +24,7 @@ - + 0.0 @@ -41,43 +42,58 @@ - - robotiq_85_left_knuckle_joint + + ${prefix}robotiq_85_left_knuckle_joint 1.0 - + + 0.0 + 0.80285 + - - robotiq_85_left_knuckle_joint + + ${prefix}robotiq_85_left_knuckle_joint 1.0 - + + 0.0 + 0.80285 + - - robotiq_85_left_knuckle_joint + + ${prefix}robotiq_85_left_knuckle_joint 1.0 - + + 0.0 + 0.80285 + - - robotiq_85_left_knuckle_joint + + ${prefix}robotiq_85_left_knuckle_joint -1.0 - + + -0.80285 + 0.0 + - - robotiq_85_left_knuckle_joint + + ${prefix}robotiq_85_left_knuckle_joint -1.0 - + + -0.80285 + 0.0 + From 489b7a50a50b6c195f5e64e644231ab227daee3d Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 01:22:59 +0200 Subject: [PATCH 52/88] Add more timeout planification solver and attempts --- Industrial/ur3_gripper_moveit_config /config/kinematics.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml b/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml index ff0bdc33f..16412cd0b 100644 --- a/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml +++ b/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml @@ -3,7 +3,7 @@ ur3_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 + kinematics_solver_timeout: 0.1 + kinematics_solver_attempts: 10 gripper: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin \ No newline at end of file From f743f365dc6c84274451452aa208cf2066652db7 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 08:06:35 +0200 Subject: [PATCH 53/88] Add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 35 +++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 02b7a18d7..054e3ecf8 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -169,13 +169,42 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z = GOAL->qz; TARGET_POSE.orientation.w = GOAL->qw; - move_group_interface_ROB.setPoseTarget(TARGET_POSE); + move_group_interface_ROB.stop(); - move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.clearPoseTargets(); + move_group_interface_ROB.clearPathConstraints(); + + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); + + auto current_state = move_group_interface_ROB.getCurrentState(); + + std::vector joints; + current_state->copyJointGroupPositions( + current_state->getJointModelGroup(param_ROB_GROUP), + joints); + + RCLCPP_INFO(get_logger(), "Current joints:"); + + for (size_t i = 0; i < joints.size(); ++i) + { + RCLCPP_INFO(get_logger(), "joint[%zu] = %.6f", i, joints[i]); + } + + RCLCPP_INFO(get_logger(), + "Target pose: %.3f %.3f %.3f | %.3f %.3f %.3f %.3f", + TARGET_POSE.position.x, + TARGET_POSE.position.y, + TARGET_POSE.position.z, + TARGET_POSE.orientation.x, + TARGET_POSE.orientation.y, + TARGET_POSE.orientation.z, + TARGET_POSE.orientation.w); + MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ @@ -282,6 +311,8 @@ int main(int argc, char **argv) move_group_interface_ROB.setMaxVelocityScalingFactor(1.0); move_group_interface_ROB.setMaxAccelerationScalingFactor(1.0); + move_group_interface_ROB.setPlanningTime(5.0); + move_group_interface_ROB.setNumPlanningAttempts(10); RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", param_ROB_GROUP.c_str()); From c90b4efd6beb74e3a5e24869b1f471119fae12b5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 08:27:13 +0200 Subject: [PATCH 54/88] Change xacro and ros2control for gripper --- .../urdf/robotiq_2f85_macro.urdf.xacro | 357 +++++++----------- .../urdf/robotiq_2f85_ros2control.xacro | 54 ++- 2 files changed, 148 insertions(+), 263 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro index d05860bfc..9a5f56205 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro @@ -1,89 +1,17 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + - - 0.9 - 0.9 - Gazebo/Black - - - @@ -92,25 +20,35 @@ - + + iyx = "0.000000" iyy = "0.001110" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.001171" /> - - - - + + + + + + + + + + + + + + + - @@ -119,31 +57,18 @@ - + - - - + + + - - - - - - - - - - - - + - - @@ -152,30 +77,30 @@ - + - - - + + + + + + + + - - - - - - - - + + + + - @@ -184,26 +109,18 @@ - + + iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.000020" /> - - - - - - - - + - @@ -212,26 +129,36 @@ - + + iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.000020" /> - + - - - - - + + + + + + + + + + + + + + + + - @@ -240,29 +167,17 @@ - + + iyx = "0.000000" iyy = "0.000005" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.000035" /> - - - - - - - - - - - - @@ -271,29 +186,37 @@ - + + iyx = "0.000000" iyy = "0.000005" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.000035" /> - - - + + + - - - - + + + - + + + + + + + + + + @@ -301,32 +224,32 @@ + - - - + + iyx = "0.000000" iyy = "0.000005" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.000006" /> - - - - - - - - - + + + true + 100 + + + robotiq_85_left_finger_tip_link_collision + + + + - @@ -334,61 +257,33 @@ + - - - + - - - - - - - - - - - - - - - - - - - - + iyx = "0.000000" iyy = "0.000005" iyz = "0.000000" + izx = "0.000000" izy = "0.000000" izz = "0.000006" /> - - - - - - - - - - - - - - - - - - - true + + + true + 100 + + + robotiq_85_right_finger_tip_link_collision + + + + + - + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro index 94c18e692..32637b44a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro @@ -1,53 +1,45 @@ - + + - - gz_ros2_control/GazeboSimSystem - mock_components/GenericSystem + false + 0.0 - - - - + + - 0.0 0.80285 - 0.0 - - - - + ${prefix}robotiq_85_left_knuckle_joint 1.0 - 0.0 - 0.80285 + 0.0 + 0.80285 @@ -58,8 +50,8 @@ ${prefix}robotiq_85_left_knuckle_joint 1.0 - 0.0 - 0.80285 + 0.0 + 0.80285 @@ -69,8 +61,8 @@ ${prefix}robotiq_85_left_knuckle_joint 1.0 - 0.0 - 0.80285 + 0.0 + 0.80285 @@ -80,8 +72,8 @@ ${prefix}robotiq_85_left_knuckle_joint -1.0 - -0.80285 - 0.0 + -0.80285 + 0.0 @@ -91,15 +83,13 @@ ${prefix}robotiq_85_left_knuckle_joint -1.0 - -0.80285 - 0.0 + -0.80285 + 0.0 - - - \ No newline at end of file + \ No newline at end of file From 720c5875f8697b8298207bb839aa48045c2ffe14 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 08:40:09 +0200 Subject: [PATCH 55/88] Fix xacro for use new gripper macro --- .../robot_arms/models/ur3/ur3.urdf.xacro | 38 +++++++++++++------ 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 9964bd5ee..7e354909f 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -39,10 +39,33 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - From b3921d60a8cde0e756e3521dd168c65dc1963f68 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 09:01:29 +0200 Subject: [PATCH 56/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 054e3ecf8..b931943c4 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -148,6 +148,19 @@ class ActionServer : public rclcpp::Node RCLCPP_INFO(get_logger(), "POSITION -> (x: %.3f, y: %.3f, z: %.3f)", CP_INFO.pose.position.x, CP_INFO.pose.position.y, CP_INFO.pose.position.z); RCLCPP_INFO(get_logger(), "ORIENTATION -> (qx: %.3f, qy: %.3f, qz: %.3f, qw: %.3f)", CP_INFO.pose.orientation.x, CP_INFO.pose.orientation.y, CP_INFO.pose.orientation.z, CP_INFO.pose.orientation.w); + auto joints = move_group_interface_ROB.getCurrentJointValues(); + + RCLCPP_INFO(get_logger(), "MoveGroup current joints:"); + + for (size_t i = 0; i < joints.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "joint[%zu] = %.6f", + i, + joints[i]); + } + // 1. OBTAIN INPUT PARAMETERS: const auto GOAL = goal_handle->get_goal(); @@ -158,6 +171,19 @@ class ActionServer : public rclcpp::Node moveit::planning_interface::MoveGroupInterface::Plan MyPlan; + auto joints = move_group_interface_ROB.getCurrentJointValues(); + + RCLCPP_INFO(get_logger(), "MoveGroup current joints:"); + + for (size_t i = 0; i < joints.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "MG joint[%zu] = %.6f", + i, + joints[i]); + } + auto CURRENT_POSE = move_group_interface_ROB.getCurrentPose(); geometry_msgs::msg::Pose TARGET_POSE; @@ -174,6 +200,9 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.clearPoseTargets(); move_group_interface_ROB.clearPathConstraints(); + move_group_interface_ROB.getCurrentState(10.0); + move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); @@ -314,6 +343,15 @@ int main(int argc, char **argv) move_group_interface_ROB.setPlanningTime(5.0); move_group_interface_ROB.setNumPlanningAttempts(10); + if (!move_group_interface_ROB.getCurrentState(10.0)) + { + RCLCPP_ERROR(logger, "Failed to get current state!"); + } + else + { + RCLCPP_INFO(logger, "Current state received."); + } + RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", param_ROB_GROUP.c_str()); rclcpp::spin(node); From 3ceddf46f8bb1d71e11a92819cfff80a46874a78 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 09:09:38 +0200 Subject: [PATCH 57/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 39 ++++++++++--------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index b931943c4..22faa95fa 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -171,19 +171,6 @@ class ActionServer : public rclcpp::Node moveit::planning_interface::MoveGroupInterface::Plan MyPlan; - auto joints = move_group_interface_ROB.getCurrentJointValues(); - - RCLCPP_INFO(get_logger(), "MoveGroup current joints:"); - - for (size_t i = 0; i < joints.size(); ++i) - { - RCLCPP_INFO( - get_logger(), - "MG joint[%zu] = %.6f", - i, - joints[i]); - } - auto CURRENT_POSE = move_group_interface_ROB.getCurrentPose(); geometry_msgs::msg::Pose TARGET_POSE; @@ -212,11 +199,6 @@ class ActionServer : public rclcpp::Node auto current_state = move_group_interface_ROB.getCurrentState(); - std::vector joints; - current_state->copyJointGroupPositions( - current_state->getJointModelGroup(param_ROB_GROUP), - joints); - RCLCPP_INFO(get_logger(), "Current joints:"); for (size_t i = 0; i < joints.size(); ++i) @@ -234,6 +216,27 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z, TARGET_POSE.orientation.w); + auto state = move_group_interface_ROB.getCurrentState(); + if (state) + { + std::vector start; + + state->copyJointGroupPositions( + state->getJointModelGroup(param_ROB_GROUP), + start); + + RCLCPP_INFO(get_logger(), "Planning start state:"); + + for (size_t i = 0; i < start.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "start[%zu] = %.6f", + i, + start[i]); + } + } + MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ From 8df554c1fdb2e365c28d065c37ce66e1e2d0e092 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 09:44:16 +0200 Subject: [PATCH 58/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 48 ++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 22faa95fa..5f702469d 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -195,7 +195,53 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.setPlannerId(GOAL->type); move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed); - move_group_interface_ROB.setPoseTarget(TARGET_POSE); + moveit::core::RobotStatePtr state = + move_group_interface_ROB.getCurrentState(); + + auto state = move_group_interface_ROB.getCurrentState(); + + std::vector seed; + state->copyJointGroupPositions( + state->getJointModelGroup(param_ROB_GROUP), + seed); + + RCLCPP_INFO(get_logger(),"IK seed:"); + + for(size_t i=0;i %.6f", + i, + seed[i]); + } + + bool found = state->setFromIK( + state->getJointModelGroup(param_ROB_GROUP), + TARGET_POSE); + + if (!found) + { + RCLCPP_ERROR(get_logger(), "IK failed!"); + RES = "PLANNING: ERROR"; + return; + } + + std::vector ik_joints; + state->copyJointGroupPositions( + state->getJointModelGroup(param_ROB_GROUP), + ik_joints); + + RCLCPP_INFO(get_logger(), "IK solution:"); + + for (size_t i = 0; i < ik_joints.size(); i++) + { + RCLCPP_INFO(get_logger(), + "ik[%zu] = %.6f", + i, + ik_joints[i]); + } + + move_group_interface_ROB.setJointValueTarget(*state); auto current_state = move_group_interface_ROB.getCurrentState(); From c141b604581825f3258183024e2cb6943f05eef9 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 09:54:58 +0200 Subject: [PATCH 59/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 5f702469d..893535f78 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -198,8 +198,6 @@ class ActionServer : public rclcpp::Node moveit::core::RobotStatePtr state = move_group_interface_ROB.getCurrentState(); - auto state = move_group_interface_ROB.getCurrentState(); - std::vector seed; state->copyJointGroupPositions( state->getJointModelGroup(param_ROB_GROUP), From d4eea2ac8e9749e9fc58e9543b4863a7a9af2bd4 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 10:00:15 +0200 Subject: [PATCH 60/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 893535f78..a51efef02 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -260,7 +260,6 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z, TARGET_POSE.orientation.w); - auto state = move_group_interface_ROB.getCurrentState(); if (state) { std::vector start; From 7bfb22a456bf520cfaac9fa98c35a32c6aa2fe98 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 10:11:34 +0200 Subject: [PATCH 61/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index a51efef02..0362c80bb 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -239,7 +239,7 @@ class ActionServer : public rclcpp::Node ik_joints[i]); } - move_group_interface_ROB.setJointValueTarget(*state); + move_group_interface_ROB.setJointValueTarget(ik_joints); auto current_state = move_group_interface_ROB.getCurrentState(); @@ -280,6 +280,23 @@ class ActionServer : public rclcpp::Node } } + moveit_msgs::msg::Constraints goal = + move_group_interface_ROB.getGoalConstraints(); + + RCLCPP_INFO( + get_logger(), + "Goal constraints: %zu joint constraints", + goal.joint_constraints.size()); + + for (const auto &jc : goal.joint_constraints) + { + RCLCPP_INFO( + get_logger(), + "%s -> %.6f", + jc.joint_name.c_str(), + jc.position); + } + MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ From c8f80f6728f17e396244b29dc412281efc745c13 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 10:59:33 +0200 Subject: [PATCH 62/88] Fix robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 24 +++---------------- 1 file changed, 3 insertions(+), 21 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 0362c80bb..b3a77da88 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -238,8 +238,9 @@ class ActionServer : public rclcpp::Node i, ik_joints[i]); } + + move_group_interface_ROB.setJointValueTarget(*state); - move_group_interface_ROB.setJointValueTarget(ik_joints); auto current_state = move_group_interface_ROB.getCurrentState(); @@ -279,24 +280,6 @@ class ActionServer : public rclcpp::Node start[i]); } } - - moveit_msgs::msg::Constraints goal = - move_group_interface_ROB.getGoalConstraints(); - - RCLCPP_INFO( - get_logger(), - "Goal constraints: %zu joint constraints", - goal.joint_constraints.size()); - - for (const auto &jc : goal.joint_constraints) - { - RCLCPP_INFO( - get_logger(), - "%s -> %.6f", - jc.joint_name.c_str(), - jc.position); - } - MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ @@ -420,5 +403,4 @@ int main(int argc, char **argv) rclcpp::spin(node); rclcpp::shutdown(); - return 0; -} \ No newline at end of file + return 0; \ No newline at end of file From 6799277d00cdcc90f9bd136ce99e32b572fb010c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 11:06:15 +0200 Subject: [PATCH 63/88] Fix robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index b3a77da88..9de4f3b50 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -403,4 +403,5 @@ int main(int argc, char **argv) rclcpp::spin(node); rclcpp::shutdown(); - return 0; \ No newline at end of file + return 0; +} \ No newline at end of file From 487cd1d9a6448cf6bbaaa909962baeae3efe82a6 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 26 Jun 2026 11:46:40 +0200 Subject: [PATCH 64/88] add more debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 9de4f3b50..7a0cce2e7 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -389,6 +389,7 @@ int main(int argc, char **argv) move_group_interface_ROB.setPlanningTime(5.0); move_group_interface_ROB.setNumPlanningAttempts(10); + // Esperar a tener el estado actual if (!move_group_interface_ROB.getCurrentState(10.0)) { RCLCPP_ERROR(logger, "Failed to get current state!"); @@ -396,6 +397,18 @@ int main(int argc, char **argv) else { RCLCPP_INFO(logger, "Current state received."); + + // ===== IMPRIMIR LOS JOINTS DEL GRUPO ===== + const auto *jmg = + move_group_interface_ROB.getCurrentState() + ->getJointModelGroup(param_ROB_GROUP); + + RCLCPP_INFO(logger, "Joints in planning group '%s':", param_ROB_GROUP.c_str()); + + for (const auto &name : jmg->getVariableNames()) + { + RCLCPP_INFO(logger, " %s", name.c_str()); + } } RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", param_ROB_GROUP.c_str()); From 711f80da2e3664bc338eecccf6e4d889a6d1b2a2 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 18:03:41 +0200 Subject: [PATCH 65/88] Restart robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 148 ++---------------- 1 file changed, 14 insertions(+), 134 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 7a0cce2e7..7cae7b567 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -56,6 +56,7 @@ std::string param_ROB = "none"; // Declaration of GLOBAL VARIABLE --> RES: std::string RES = "none"; +// Declaration of GLOBAL VARIABLE --> ROBOT GROUP: std::string param_ROB_GROUP = "none"; // =============================================================================== // @@ -97,8 +98,12 @@ class ActionServer : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str()); this->declare_parameter("ROB_GROUP", "none"); - param_ROB_GROUP = - this->get_parameter("ROB_GROUP").as_string(); + param_ROB_GROUP = this->get_parameter("ROB_GROUP").as_string(); + + RCLCPP_INFO( + this->get_logger(), + "ROB_GROUP received -> %s", + param_ROB_GROUP.c_str()); action_server_ = rclcpp_action::create_server( this, @@ -148,19 +153,6 @@ class ActionServer : public rclcpp::Node RCLCPP_INFO(get_logger(), "POSITION -> (x: %.3f, y: %.3f, z: %.3f)", CP_INFO.pose.position.x, CP_INFO.pose.position.y, CP_INFO.pose.position.z); RCLCPP_INFO(get_logger(), "ORIENTATION -> (qx: %.3f, qy: %.3f, qz: %.3f, qw: %.3f)", CP_INFO.pose.orientation.x, CP_INFO.pose.orientation.y, CP_INFO.pose.orientation.z, CP_INFO.pose.orientation.w); - auto joints = move_group_interface_ROB.getCurrentJointValues(); - - RCLCPP_INFO(get_logger(), "MoveGroup current joints:"); - - for (size_t i = 0; i < joints.size(); ++i) - { - RCLCPP_INFO( - get_logger(), - "joint[%zu] = %.6f", - i, - joints[i]); - } - // 1. OBTAIN INPUT PARAMETERS: const auto GOAL = goal_handle->get_goal(); @@ -182,104 +174,13 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z = GOAL->qz; TARGET_POSE.orientation.w = GOAL->qw; - move_group_interface_ROB.stop(); - - move_group_interface_ROB.clearPoseTargets(); - move_group_interface_ROB.clearPathConstraints(); - - move_group_interface_ROB.getCurrentState(10.0); - move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); - move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed); - moveit::core::RobotStatePtr state = - move_group_interface_ROB.getCurrentState(); - - std::vector seed; - state->copyJointGroupPositions( - state->getJointModelGroup(param_ROB_GROUP), - seed); - - RCLCPP_INFO(get_logger(),"IK seed:"); - - for(size_t i=0;i %.6f", - i, - seed[i]); - } - - bool found = state->setFromIK( - state->getJointModelGroup(param_ROB_GROUP), - TARGET_POSE); - - if (!found) - { - RCLCPP_ERROR(get_logger(), "IK failed!"); - RES = "PLANNING: ERROR"; - return; - } - - std::vector ik_joints; - state->copyJointGroupPositions( - state->getJointModelGroup(param_ROB_GROUP), - ik_joints); - - RCLCPP_INFO(get_logger(), "IK solution:"); - - for (size_t i = 0; i < ik_joints.size(); i++) - { - RCLCPP_INFO(get_logger(), - "ik[%zu] = %.6f", - i, - ik_joints[i]); - } - - move_group_interface_ROB.setJointValueTarget(*state); - - - auto current_state = move_group_interface_ROB.getCurrentState(); - - RCLCPP_INFO(get_logger(), "Current joints:"); - - for (size_t i = 0; i < joints.size(); ++i) - { - RCLCPP_INFO(get_logger(), "joint[%zu] = %.6f", i, joints[i]); - } - - RCLCPP_INFO(get_logger(), - "Target pose: %.3f %.3f %.3f | %.3f %.3f %.3f %.3f", - TARGET_POSE.position.x, - TARGET_POSE.position.y, - TARGET_POSE.position.z, - TARGET_POSE.orientation.x, - TARGET_POSE.orientation.y, - TARGET_POSE.orientation.z, - TARGET_POSE.orientation.w); - - if (state) - { - std::vector start; - - state->copyJointGroupPositions( - state->getJointModelGroup(param_ROB_GROUP), - start); - - RCLCPP_INFO(get_logger(), "Planning start state:"); - - for (size_t i = 0; i < start.size(); ++i) - { - RCLCPP_INFO( - get_logger(), - "start[%zu] = %.6f", - i, - start[i]); - } - } MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ @@ -382,36 +283,15 @@ int main(int argc, char **argv) using moveit::planning_interface::MoveGroupInterface; move_group_interface_ROB = - MoveGroupInterface(moveit_node, param_ROB_GROUP); + MoveGroupInterface(moveit_node, param_ROB_GROUP); move_group_interface_ROB.setMaxVelocityScalingFactor(1.0); move_group_interface_ROB.setMaxAccelerationScalingFactor(1.0); - move_group_interface_ROB.setPlanningTime(5.0); - move_group_interface_ROB.setNumPlanningAttempts(10); - - // Esperar a tener el estado actual - if (!move_group_interface_ROB.getCurrentState(10.0)) - { - RCLCPP_ERROR(logger, "Failed to get current state!"); - } - else - { - RCLCPP_INFO(logger, "Current state received."); - - // ===== IMPRIMIR LOS JOINTS DEL GRUPO ===== - const auto *jmg = - move_group_interface_ROB.getCurrentState() - ->getJointModelGroup(param_ROB_GROUP); - - RCLCPP_INFO(logger, "Joints in planning group '%s':", param_ROB_GROUP.c_str()); - - for (const auto &name : jmg->getVariableNames()) - { - RCLCPP_INFO(logger, " %s", name.c_str()); - } - } - RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", param_ROB_GROUP.c_str()); + RCLCPP_INFO( + logger, + "MoveGroupInterface object created for ROBOT: %s", + param_ROB_GROUP.c_str()); rclcpp::spin(node); From 36013caabc1d7415887a7bbbe82ff8a95a5c08fc Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 18:41:28 +0200 Subject: [PATCH 66/88] Add disable collision to srdf --- .../config/ur3robotiq_2f85.srdf | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf index f4455b9bb..eb5f1a064 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf @@ -199,4 +199,21 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file From 752938b05b0ff3cfce0fdfe376542df0e9a5aba3 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 19:02:38 +0200 Subject: [PATCH 67/88] Try to use robot alone in world and spawn at 0 0 0 --- Worlds/sausage_exercise.world | 36 ----------------------------------- database/universes.sql | 2 +- 2 files changed, 1 insertion(+), 37 deletions(-) diff --git a/Worlds/sausage_exercise.world b/Worlds/sausage_exercise.world index 886f00a84..36e514d7d 100644 --- a/Worlds/sausage_exercise.world +++ b/Worlds/sausage_exercise.world @@ -39,7 +39,6 @@ true - @@ -66,41 +65,6 @@ - - model://conveyor_belt - 0 0 0 0 0 0 - - - - - true - -0.5 0 0 0 0 0 - - 0 0 0.45 0 0 0 - - - - 0.09 - 0.9 - - - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - - - - - - 0.09 - 0.9 - - - - - - \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 9fba468a2..710d5a43e 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {-1.0,10.0,0.1,0.0,0.0,0.0} 68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 70 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {0.0,0.0,0.9,0.0,0.0,0.0} -71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz {-0.5,0.0,0.9,0.0,0.0,0.0} +71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} \. -- From 7c4db03a9c4abe86d8dd232f71ae9b18c0dc8b64 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 19:18:57 +0200 Subject: [PATCH 68/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 43 ++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 7cae7b567..f12ab86ba 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -176,11 +176,39 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.setPoseTarget(TARGET_POSE); - move_group_interface_ROB.setStartStateToCurrentState(); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed); + // ===== DEBUG ===== + auto joints = move_group_interface_ROB.getCurrentJointValues(); + + RCLCPP_INFO(get_logger(), "CURRENT JOINTS:"); + + for (size_t i = 0; i < joints.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "CURRENT[%zu] = %.6f", + i, + joints[i]); + } + + auto target = move_group_interface_ROB.getJointValueTarget(); + + RCLCPP_INFO(get_logger(), "TARGET JOINTS:"); + + for (size_t i = 0; i < target.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "TARGET[%zu] = %.6f", + i, + target[i]); + } + // ================== + MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ @@ -207,6 +235,19 @@ class ActionServer : public rclcpp::Node bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + auto end_joints = move_group_interface_ROB.getCurrentJointValues(); + + RCLCPP_INFO(get_logger(), "FINAL JOINTS:"); + + for (size_t i = 0; i < end_joints.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "FINAL[%zu] = %.6f", + i, + end_joints[i]); + } + if (goal_handle->is_canceling()) { RCLCPP_INFO(this->get_logger(), "ROBOT MOVEMENT (%s) has been CANCELED.", GOAL->type.c_str()); RESULT->success = false; From 94bedccbf2596023a5fba67827dd2c897f0703f5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 19:28:30 +0200 Subject: [PATCH 69/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 55 ++++++++++++++----- 1 file changed, 42 insertions(+), 13 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index f12ab86ba..f5d3b6bb9 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -174,8 +174,50 @@ class ActionServer : public rclcpp::Node TARGET_POSE.orientation.z = GOAL->qz; TARGET_POSE.orientation.w = GOAL->qw; + RCLCPP_INFO( + get_logger(), + "TARGET POSE = %.3f %.3f %.3f", + TARGET_POSE.position.x, + TARGET_POSE.position.y, + TARGET_POSE.position.z); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); + moveit::core::RobotStatePtr state = + move_group_interface_ROB.getCurrentState(); + + const moveit::core::JointModelGroup* jmg = + state->getJointModelGroup(param_ROB_GROUP); + + bool found_ik = state->setFromIK(jmg, TARGET_POSE); + + RCLCPP_INFO(get_logger(), "IK FOUND = %d", found_ik); + + if (!found_ik) + { + RCLCPP_ERROR(get_logger(), "NO IK SOLUTION"); + } + + if (found_ik) + { + std::vector vals; + state->copyJointGroupPositions(jmg, vals); + + for (size_t i = 0; i < vals.size(); ++i) + RCLCPP_INFO(get_logger(), "IK[%zu] = %.6f", i, vals[i]); + } + + state->update(); + + const auto &tf = state->getGlobalLinkTransform("tool0"); + + RCLCPP_INFO( + get_logger(), + "IK TOOL = %.3f %.3f %.3f", + tf.translation().x(), + tf.translation().y(), + tf.translation().z()); + move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); @@ -194,19 +236,6 @@ class ActionServer : public rclcpp::Node i, joints[i]); } - - auto target = move_group_interface_ROB.getJointValueTarget(); - - RCLCPP_INFO(get_logger(), "TARGET JOINTS:"); - - for (size_t i = 0; i < target.size(); ++i) - { - RCLCPP_INFO( - get_logger(), - "TARGET[%zu] = %.6f", - i, - target[i]); - } // ================== MyPlan = plan_ROB(); From 102e42ca90361a398ea11444e694c247de2142eb Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 19:44:45 +0200 Subject: [PATCH 70/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index f5d3b6bb9..16dea1085 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -236,6 +236,26 @@ class ActionServer : public rclcpp::Node i, joints[i]); } + + // Estado interno de MoveIt + auto planning_state = move_group_interface_ROB.getCurrentState(); + + std::vector planning_vals; + + planning_state->copyJointGroupPositions( + planning_state->getJointModelGroup(param_ROB_GROUP), + planning_vals); + + RCLCPP_INFO(get_logger(), "PLANNING STATE:"); + + for (size_t i = 0; i < planning_vals.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "STATE[%zu] = %.6f", + i, + planning_vals[i]); + } // ================== MyPlan = plan_ROB(); From 18313841a80f35888600c71d9ee9cee850776982 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 19:59:46 +0200 Subject: [PATCH 71/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 36 +++++++++++++++---- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 16dea1085..9effdeb1a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -148,10 +148,24 @@ class ActionServer : public rclcpp::Node { // 0. INFORMATION -> Current Robot Pose: - auto CP_INFO = move_group_interface_ROB.getCurrentPose(); - RCLCPP_INFO(get_logger(), "INFORMATION -> Current Robot Pose:"); - RCLCPP_INFO(get_logger(), "POSITION -> (x: %.3f, y: %.3f, z: %.3f)", CP_INFO.pose.position.x, CP_INFO.pose.position.y, CP_INFO.pose.position.z); - RCLCPP_INFO(get_logger(), "ORIENTATION -> (qx: %.3f, qy: %.3f, qz: %.3f, qw: %.3f)", CP_INFO.pose.orientation.x, CP_INFO.pose.orientation.y, CP_INFO.pose.orientation.z, CP_INFO.pose.orientation.w); + auto CP_INFO = move_group_interface_ROB.getCurrentPose(); + + auto state = move_group_interface_ROB.getCurrentState(); + state->update(); + + const auto& tf = state->getGlobalLinkTransform("tool0"); + + RCLCPP_INFO(get_logger(), + "CURRENTPOSE = %.3f %.3f %.3f", + CP_INFO.pose.position.x, + CP_INFO.pose.position.y, + CP_INFO.pose.position.z); + + RCLCPP_INFO(get_logger(), + "STATE TOOL = %.3f %.3f %.3f", + tf.translation().x(), + tf.translation().y(), + tf.translation().z()); // 1. OBTAIN INPUT PARAMETERS: const auto GOAL = goal_handle->get_goal(); @@ -183,14 +197,22 @@ class ActionServer : public rclcpp::Node move_group_interface_ROB.setPoseTarget(TARGET_POSE); - moveit::core::RobotStatePtr state = - move_group_interface_ROB.getCurrentState(); - const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(param_ROB_GROUP); bool found_ik = state->setFromIK(jmg, TARGET_POSE); + state->update(); + + const auto& ik_tf = state->getGlobalLinkTransform("tool0"); + + RCLCPP_INFO( + get_logger(), + "IK TOOL = %.3f %.3f %.3f", + ik_tf.translation().x(), + ik_tf.translation().y(), + ik_tf.translation().z()); + RCLCPP_INFO(get_logger(), "IK FOUND = %d", found_ik); if (!found_ik) From fe942ca55fecb831465e5a74abec77cbd3393912 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 20:06:47 +0200 Subject: [PATCH 72/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 9effdeb1a..1c858c858 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -153,7 +153,7 @@ class ActionServer : public rclcpp::Node auto state = move_group_interface_ROB.getCurrentState(); state->update(); - const auto& tf = state->getGlobalLinkTransform("tool0"); + const auto& current_tf = state->getGlobalLinkTransform("tool0"); RCLCPP_INFO(get_logger(), "CURRENTPOSE = %.3f %.3f %.3f", @@ -162,10 +162,10 @@ class ActionServer : public rclcpp::Node CP_INFO.pose.position.z); RCLCPP_INFO(get_logger(), - "STATE TOOL = %.3f %.3f %.3f", - tf.translation().x(), - tf.translation().y(), - tf.translation().z()); + "STATE TOOL = %.3f %.3f %.3f", + current_tf.translation().x(), + current_tf.translation().y(), + current_tf.translation().z()); // 1. OBTAIN INPUT PARAMETERS: const auto GOAL = goal_handle->get_goal(); From 8891429b41e678a76d404b243266026783b2734c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 20:26:30 +0200 Subject: [PATCH 73/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 1c858c858..318b843c1 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -65,7 +65,14 @@ std::string param_ROB_GROUP = "none"; moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() { moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_interface_ROB.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + auto code = move_group_interface_ROB.plan(my_plan); + + RCLCPP_INFO( + rclcpp::get_logger("RobMove"), + "MoveIt error code = %d", + code.val); + + bool success = (code == moveit::core::MoveItErrorCode::SUCCESS); // Execute the plan if (success) From 3908e7baa670f475584db26144c20a1f365c4d16 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 20:46:47 +0200 Subject: [PATCH 74/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 318b843c1..1d57e9ed8 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -202,7 +202,8 @@ class ActionServer : public rclcpp::Node TARGET_POSE.position.y, TARGET_POSE.position.z); - move_group_interface_ROB.setPoseTarget(TARGET_POSE); + move_group_interface_ROB.clearPoseTargets(); + move_group_interface_ROB.clearPathConstraints(); const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(param_ROB_GROUP); @@ -234,6 +235,29 @@ class ActionServer : public rclcpp::Node for (size_t i = 0; i < vals.size(); ++i) RCLCPP_INFO(get_logger(), "IK[%zu] = %.6f", i, vals[i]); + + bool ok = move_group_interface_ROB.setJointValueTarget(vals); + + RCLCPP_INFO( + get_logger(), + "setJointValueTarget() = %d", + ok); + + move_group_interface_ROB.setJointValueTarget(vals); + + std::vector target; + move_group_interface_ROB.getJointValueTarget(target); + + RCLCPP_INFO(get_logger(), "TARGET JOINTS:"); + + for (size_t i = 0; i < target.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "TARGET[%zu] = %.6f", + i, + target[i]); + } } state->update(); @@ -247,6 +271,7 @@ class ActionServer : public rclcpp::Node tf.translation().y(), tf.translation().z()); + move_group_interface_ROB.setPoseTarget(TARGET_POSE); move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); @@ -313,6 +338,9 @@ class ActionServer : public rclcpp::Node bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + move_group_interface_ROB.clearPoseTargets(); + move_group_interface_ROB.clearPathConstraints(); + auto end_joints = move_group_interface_ROB.getCurrentJointValues(); RCLCPP_INFO(get_logger(), "FINAL JOINTS:"); From 77cab43a0cfc538d890840fa738d375249c63665 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 21:15:44 +0200 Subject: [PATCH 75/88] Add angle normalize to planification --- .../ros2srrc_execution/src/robmove.cpp | 62 ++++++++++++++++--- 1 file changed, 52 insertions(+), 10 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 1d57e9ed8..2d52235d6 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -59,6 +59,21 @@ std::string RES = "none"; // Declaration of GLOBAL VARIABLE --> ROBOT GROUP: std::string param_ROB_GROUP = "none"; +#include + +void normalizeToNearest(std::vector& target, + const std::vector& current) +{ + for (size_t i = 0; i < target.size(); ++i) + { + while (target[i] - current[i] > M_PI) + target[i] -= 2.0 * M_PI; + + while (target[i] - current[i] < -M_PI) + target[i] += 2.0 * M_PI; + } +} + // =============================================================================== // // MoveIt!2 -> MoveGroupInterface/Plan function: @@ -233,27 +248,54 @@ class ActionServer : public rclcpp::Node std::vector vals; state->copyJointGroupPositions(jmg, vals); + // Estado actual del robot + std::vector current = + move_group_interface_ROB.getCurrentJointValues(); + + RCLCPP_INFO(get_logger(), "CURRENT JOINTS:"); + for (size_t i = 0; i < current.size(); ++i) + { + RCLCPP_INFO(get_logger(), + "CURRENT[%zu] = %.6f", + i, + current[i]); + } + + RCLCPP_INFO(get_logger(), "IK BEFORE NORMALIZATION:"); + for (size_t i = 0; i < vals.size(); ++i) + { + RCLCPP_INFO(get_logger(), + "IK[%zu] = %.6f", + i, + vals[i]); + } + + // -------- NORMALIZACIÓN -------- + normalizeToNearest(vals, current); + + RCLCPP_INFO(get_logger(), "IK AFTER NORMALIZATION:"); for (size_t i = 0; i < vals.size(); ++i) - RCLCPP_INFO(get_logger(), "IK[%zu] = %.6f", i, vals[i]); + { + RCLCPP_INFO(get_logger(), + "IKN[%zu] = %.6f", + i, + vals[i]); + } + // Pasar esos joints al planificador bool ok = move_group_interface_ROB.setJointValueTarget(vals); - RCLCPP_INFO( - get_logger(), - "setJointValueTarget() = %d", + RCLCPP_INFO(get_logger(), + "setJointValueTarget = %d", ok); - move_group_interface_ROB.setJointValueTarget(vals); - std::vector target; move_group_interface_ROB.getJointValueTarget(target); RCLCPP_INFO(get_logger(), "TARGET JOINTS:"); - for (size_t i = 0; i < target.size(); ++i) { - RCLCPP_INFO( - get_logger(), + RCLCPP_INFO(get_logger(), "TARGET[%zu] = %.6f", i, target[i]); @@ -271,7 +313,7 @@ class ActionServer : public rclcpp::Node tf.translation().y(), tf.translation().z()); - move_group_interface_ROB.setPoseTarget(TARGET_POSE); + //move_group_interface_ROB.setPoseTarget(TARGET_POSE); move_group_interface_ROB.setStartStateToCurrentState(); move_group_interface_ROB.setPlannerId(GOAL->type); From b692a2627de8cda4431adc6e596173bead5c5491 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 21:29:44 +0200 Subject: [PATCH 76/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 2d52235d6..f02be1fe3 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -46,6 +46,7 @@ #include #include #include +#include // Declaration of GLOBAL VARIABLE --> MoveIt!2 Interface: moveit::planning_interface::MoveGroupInterface move_group_interface_ROB; @@ -357,6 +358,30 @@ class ActionServer : public rclcpp::Node MyPlan = plan_ROB(); if (RES == "PLANNING: OK"){ + + const auto& pts = MyPlan.trajectory_.joint_trajectory.points; + RCLCPP_INFO(get_logger(), "Trajectory has %zu points", pts.size()); + const auto& names = MyPlan.trajectory_.joint_trajectory.joint_names; + + std::stringstream ns; + ns << "Joint order: "; + + for (const auto& n : names) + ns << n << " "; + + RCLCPP_INFO(get_logger(), "%s", ns.str().c_str()); + + for (size_t p = 0; p < pts.size(); ++p) + { + std::stringstream ss; + ss << "Point " << p << ": "; + + for (double q : pts[p].positions) + ss << q << " "; + + RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); + } + robot_trajectory::RobotTrajectory rt( move_group_interface_ROB.getRobotModel(), param_ROB_GROUP From 0505fe130b8c0e44a0805d08fbaf3fb6ff04152e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 22:37:43 +0200 Subject: [PATCH 77/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 45 ++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index f02be1fe3..230749354 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -403,7 +403,50 @@ class ActionServer : public rclcpp::Node rt.getRobotTrajectoryMsg(MyPlan.trajectory_); - bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + // ===== DEBUG TRAJECTORY ===== + + RCLCPP_INFO(get_logger(), "Joint names:"); + + for (auto &j : MyPlan.trajectory_.joint_trajectory.joint_names) + { + RCLCPP_INFO(get_logger(), "%s", j.c_str()); + } + + RCLCPP_INFO( + get_logger(), + "Trajectory has %zu points", + MyPlan.trajectory_.joint_trajectory.points.size()); + + auto &first = MyPlan.trajectory_.joint_trajectory.points.front(); + auto &last = MyPlan.trajectory_.joint_trajectory.points.back(); + + RCLCPP_INFO(get_logger(), "FIRST POINT"); + + for (size_t i = 0; i < first.positions.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "%s = %.6f", + MyPlan.trajectory_.joint_trajectory.joint_names[i].c_str(), + first.positions[i]); + } + + RCLCPP_INFO(get_logger(), "LAST POINT"); + + for (size_t i = 0; i < last.positions.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "%s = %.6f", + MyPlan.trajectory_.joint_trajectory.joint_names[i].c_str(), + last.positions[i]); + } + + // ============================ + + bool ExecSUCCESS = + (move_group_interface_ROB.execute(MyPlan) + == moveit::planning_interface::MoveItErrorCode::SUCCESS); move_group_interface_ROB.clearPoseTargets(); move_group_interface_ROB.clearPathConstraints(); From 42df895c861f3f7a97896126bc2770327f4f466d Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 27 Jun 2026 23:41:18 +0200 Subject: [PATCH 78/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index 230749354..bf19f88cf 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -444,10 +444,40 @@ class ActionServer : public rclcpp::Node // ============================ + const auto& last = MyPlan.trajectory_.joint_trajectory.points.back(); + + RCLCPP_INFO(get_logger(), "FINAL TRAJECTORY POINT"); + + for (size_t i = 0; i < last.positions.size(); ++i) + { + RCLCPP_INFO( + get_logger(), + "%s = %.6f", + MyPlan.trajectory_.joint_trajectory.joint_names[i].c_str(), + last.positions[i]); + } + bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + auto state = move_group_interface_ROB.getCurrentState(); + + std::vector q; + state->copyJointGroupPositions( + state->getJointModelGroup(param_ROB_GROUP), + q); + + RCLCPP_INFO(get_logger(), "ROBOT AFTER EXECUTION"); + + for(size_t i=0;i Date: Sat, 27 Jun 2026 23:47:11 +0200 Subject: [PATCH 79/88] add debug to robmove.cpp --- .../ros2srrc_execution/src/robmove.cpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp index bf19f88cf..091d8c3d6 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp @@ -444,19 +444,6 @@ class ActionServer : public rclcpp::Node // ============================ - const auto& last = MyPlan.trajectory_.joint_trajectory.points.back(); - - RCLCPP_INFO(get_logger(), "FINAL TRAJECTORY POINT"); - - for (size_t i = 0; i < last.positions.size(); ++i) - { - RCLCPP_INFO( - get_logger(), - "%s = %.6f", - MyPlan.trajectory_.joint_trajectory.joint_names[i].c_str(), - last.positions[i]); - } - bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); From d10c50a7796aa6a0f44c33d1938d01fe0b8fe4a1 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 28 Jun 2026 00:14:18 +0200 Subject: [PATCH 80/88] Change joint_limits --- .../robot_arms/config/ur5/joint_limits.yaml | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/CustomRobots/robot_arms/config/ur5/joint_limits.yaml b/CustomRobots/robot_arms/config/ur5/joint_limits.yaml index e6093ea65..947574c93 100644 --- a/CustomRobots/robot_arms/config/ur5/joint_limits.yaml +++ b/CustomRobots/robot_arms/config/ur5/joint_limits.yaml @@ -15,9 +15,9 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 150.0 - max_position: !degrees 360.0 + max_position: !degrees 180.0 max_velocity: !degrees 180.0 - min_position: !degrees -360.0 + min_position: !degrees -180.0 shoulder_lift_joint: # acceleration limits are not publicly available has_acceleration_limits: false @@ -25,9 +25,9 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 150.0 - max_position: !degrees 360.0 + max_position: !degrees 180.0 max_velocity: !degrees 180.0 - min_position: !degrees -360.0 + min_position: !degrees -180.0 elbow_joint: # acceleration limits are not publicly available has_acceleration_limits: false @@ -55,9 +55,9 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 28.0 - max_position: !degrees 360.0 + max_position: !degrees 180.0 max_velocity: !degrees 180.0 - min_position: !degrees -360.0 + min_position: !degrees -180.0 wrist_2_joint: # acceleration limits are not publicly available has_acceleration_limits: false @@ -65,9 +65,9 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 28.0 - max_position: !degrees 360.0 + max_position: !degrees 180.0 max_velocity: !degrees 180.0 - min_position: !degrees -360.0 + min_position: !degrees -180.0 wrist_3_joint: # acceleration limits are not publicly available has_acceleration_limits: false @@ -75,6 +75,6 @@ joint_limits: has_position_limits: true has_velocity_limits: true max_effort: 28.0 - max_position: !degrees 360.0 + max_position: !degrees 180.0 max_velocity: !degrees 180.0 - min_position: !degrees -360.0 + min_position: !degrees -180.0 From 5af0d3483c378cc7dd2ef1beb9296f19704120fe Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 28 Jun 2026 01:06:45 +0200 Subject: [PATCH 81/88] Delete ros2_control in ur3.urdf.xaccro --- CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 7e354909f..88dcf1562 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -67,16 +67,4 @@ - - From 754996d4aa3fa33f5979852829419da503097bc8 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 28 Jun 2026 09:16:41 +0200 Subject: [PATCH 82/88] Restart susage world --- Worlds/sausage_exercise.world | 36 +++++++++++++++++++++++++++++++++++ database/universes.sql | 2 +- 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/Worlds/sausage_exercise.world b/Worlds/sausage_exercise.world index 36e514d7d..886f00a84 100644 --- a/Worlds/sausage_exercise.world +++ b/Worlds/sausage_exercise.world @@ -39,6 +39,7 @@ true + @@ -65,6 +66,41 @@ + + model://conveyor_belt + 0 0 0 0 0 0 + + + + + true + -0.5 0 0 0 0 0 + + 0 0 0.45 0 0 0 + + + + 0.09 + 0.9 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + + + + + + 0.09 + 0.9 + + + + + + \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 710d5a43e..9fba468a2 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {-1.0,10.0,0.1,0.0,0.0,0.0} 68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 70 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {0.0,0.0,0.9,0.0,0.0,0.0} -71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} +71 Conveyor Harmonic /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz {-0.5,0.0,0.9,0.0,0.0,0.0} \. -- From 57452d7c40a77f04fa0c9ca4574ad843af565431 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Mon, 29 Jun 2026 17:21:19 +0200 Subject: [PATCH 83/88] Add cams to universe --- database/universes.sql | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/database/universes.sql b/database/universes.sql index 9fba468a2..674039810 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -198,7 +198,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 68 Rover 4wd Warehouse Low Noise 68 19 69 Rover 4wd Warehouse High Noise 68 20 70 Machine vision Harmonic world 70 24 -71 Conveyor Belt World 71 25 +71 Conveyor Belt World 71 26 \. -- From 64e06d2f778b1785fd50f4611b883bd8fd6ec2b4 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 30 Jun 2026 18:30:51 +0200 Subject: [PATCH 84/88] add sausage spawn and move base camera --- CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro | 9 ++++++++- Launchers/sausage_exercise.launch.py | 9 +++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 88dcf1562..13889fe54 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -27,7 +27,14 @@ - + + + + + + + + diff --git a/Launchers/sausage_exercise.launch.py b/Launchers/sausage_exercise.launch.py index eff793839..ecc0590f9 100644 --- a/Launchers/sausage_exercise.launch.py +++ b/Launchers/sausage_exercise.launch.py @@ -6,6 +6,7 @@ from launch.actions import ExecuteProcess, SetEnvironmentVariable from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node def generate_launch_description(): @@ -70,11 +71,19 @@ def generate_launch_description(): output="screen", ) + sausage_spawner = Node( + package="conveyor_belt_plugin", + executable="sausage_spawner", + name="sausage_spawner", + output="screen", + ) + return LaunchDescription( [ set_gz_plugin_path, set_ld_library_path, set_resource_path, gz, + sausage_spawner, ] ) From 8aac1e5f6ca745d7d190e368e2eea91aa5677f01 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 30 Jun 2026 19:00:11 +0200 Subject: [PATCH 85/88] Change camera base pose --- CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 13889fe54..35e73d091 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -28,13 +28,13 @@ - - - + + + - - - + + + From fb712e077a8017864c742bb7ccc35877f0fa6806 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 30 Jun 2026 19:36:28 +0200 Subject: [PATCH 86/88] Fix sausage spawn --- Launchers/sausage_exercise.launch.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Launchers/sausage_exercise.launch.py b/Launchers/sausage_exercise.launch.py index ecc0590f9..9468f828a 100644 --- a/Launchers/sausage_exercise.launch.py +++ b/Launchers/sausage_exercise.launch.py @@ -71,10 +71,11 @@ def generate_launch_description(): output="screen", ) - sausage_spawner = Node( - package="conveyor_belt_plugin", - executable="sausage_spawner", - name="sausage_spawner", + sausage_spawner = ExecuteProcess( + cmd=[ + "python3", + "home/ws/src/CustomRobots/conveyor_belt/spawn_sausage.py" + ], output="screen", ) From 2efb62bad805cd1030a19ac89b080e1065c59528 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 1 Jul 2026 09:44:56 +0200 Subject: [PATCH 87/88] Add sensor plugin to world --- Worlds/sausage_exercise.world | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Worlds/sausage_exercise.world b/Worlds/sausage_exercise.world index 886f00a84..e90e7bc06 100644 --- a/Worlds/sausage_exercise.world +++ b/Worlds/sausage_exercise.world @@ -12,8 +12,10 @@ - + + ogre2 + 0 0 -9.8 From 8899a3a238af48faf8858500737ae2a260bac3c7 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Wed, 1 Jul 2026 10:02:20 +0200 Subject: [PATCH 88/88] Move down base camera --- CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro index 35e73d091..630267ae4 100644 --- a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro +++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro @@ -30,7 +30,7 @@ - +