From bd6d3c29bafc8d1f4fb6f6b67db46ee583a488bc Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 11:58:14 +0200 Subject: [PATCH 01/31] Try to fix spawn robot pose --- .../config/configurations.yaml | 2 +- ..._robotiq_2f85_with_cams_classic.urdf.xacro | 162 +++++ .../ur5/urdf/ur5_macro_classic.urdf.xacro | 334 +++++++++ .../ur5/urdf/ur5_ros2control_classic.xacro | 390 +++++++++++ .../machine_vision.launch.py | 638 ++++++++++++++++++ 5 files changed, 1525 insertions(+), 1 deletion(-) create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro create mode 100644 Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro create mode 100644 Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_ros2control_classic.xacro create mode 100644 Launchers/machine_vision_classic/machine_vision.launch.py diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/config/configurations.yaml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/config/configurations.yaml index 377039f36..3692bf91a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/config/configurations.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/config/configurations.yaml @@ -39,7 +39,7 @@ Configurations: - ID: "ur5_7" Name: "UR5 + Robotiq HandE gripper on top of Robot Stand." - urdf: "ur5_robotiq_2f85_with_cams.urdf.xacro" + urdf: "ur5_robotiq_2f85_with_cams_classic.urdf.xacro" rob: "ur5" ee: "robotiq_2f85" diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro new file mode 100644 index 000000000..6d2443e2e --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro @@ -0,0 +1,162 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + $(find ros2srrc_robots)/ur5/config/controller.yaml + + + + $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro new file mode 100644 index 000000000..670b09be9 --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro @@ -0,0 +1,334 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_ros2control_classic.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_ros2control_classic.xacro new file mode 100644 index 000000000..3a405fc2a --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_ros2control_classic.xacro @@ -0,0 +1,390 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + ur_robot_driver/URPositionHardwareInterface + ${robot_ip} + ${script_filename} + ${output_recipe_filename} + ${input_recipe_filename} + false + ${reverse_port} + ${script_sender_port} + ${reverse_ip} + ${script_command_port} + ${trajectory_port} + ${tf_prefix} + ${non_blocking_read} + ${servoj_gain} + ${servoj_lookahead_time} + ${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} + ${keep_alive_count} + + + + + + + + + + + + + + + + + ${-2*pi} + ${2*pi} + + + -0.9 + 0.9 + + + 0.0 + + + 0.0 + + + 0.0 + + + + + + + ${-2*pi} + ${2*pi} + + + -0.9 + 0.9 + + + ${-pi/2} + + + 0.0 + + + 0.0 + + + + + + + ${-pi} + ${pi} + + + -0.9 + 0.9 + + + 0.0 + + + 0.0 + + + 0.0 + + + + + + + ${-2*pi} + ${2*pi} + + + -1.85 + 1.85 + + + ${-pi/2} + + + 0.0 + + + 0.0 + + + + + + + ${-2*pi} + ${2*pi} + + + -1.85 + 1.85 + + + 0.0 + + + 0.0 + + + 0.0 + + + + + + + ${-2*pi} + ${2*pi} + + + -1.85 + 1.85 + + + 0.0 + + + 0.0 + + + 0.0 + + + + + + + + + + + + + + + -0.9 + 0.9 + + + 0.1 + 1.0 + + + 0.0 + + + 0.0 + + + 0.0 + + + + + + + + + false + + + false + + + false + + + false + + + false + + + false + + + + + false + + + false + + + + true + + true + + + + + + + \ No newline at end of file diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py new file mode 100644 index 000000000..91e37dda4 --- /dev/null +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -0,0 +1,638 @@ +#!/usr/bin/python3 + +# ===================================== COPYRIGHT ===================================== # +# # +# IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY # +# Created on behalf of the IFRA Group at Cranfield University, United Kingdom # +# E-mail: IFRA@cranfield.ac.uk # +# # +# Licensed under the Apache-2.0 License. # +# You may not use this file except in compliance with the License. # +# You may obtain a copy of the License at: http://www.apache.org/licenses/LICENSE-2.0 # +# # +# Unless required by applicable law or agreed to in writing, software distributed # +# under the License is distributed on an "as-is" basis, without warranties or # +# conditions of any kind, either express or implied. See the License for the specific # +# language governing permissions and limitations under the License. # +# # +# IFRA Group - Cranfield University # +# AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk # +# Dr. Seemal Asif - s.asif@cranfield.ac.uk # +# Prof. Phil Webb - p.f.webb@cranfield.ac.uk # +# # +# Date: June, 2024. # +# # +# ===================================== COPYRIGHT ===================================== # + +# ======= CITE OUR WORK ======= # +# You can cite our work with the following statement: +# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl. + +# moveit2.launch.py: +# Launch file for the Robot's GAZEBO SIMULATION + MoveIt!2 Framework in ROS2 Humble: + +# Import libraries: +import os, sys, xacro, yaml +from ament_index_python.packages import get_package_share_directory, PackageNotFoundError +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, RegisterEventHandler, TimerAction +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from moveit_configs_utils import MoveItConfigsBuilder +from launch.actions import SetEnvironmentVariable + +# LOAD FILE: +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: + # parent of IOError, OSError *and* WindowsError where available. + return None + +# LOAD YAML: +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: + # parent of IOError, OSError *and* WindowsError where available. + return None + +# ===== REQUIRED TO GET THE ROBOT CONFIGURATION === # + +# EVALUATE INPUT ARGUMENTS: +def AssignArgument(ARGUMENT): + ARGUMENTS = sys.argv + for y in ARGUMENTS: + if (ARGUMENT + ":=") in y: + ARG = y.replace((ARGUMENT + ":="),"") + return(ARG) + +# GET CONFIGURATION from YAML: +def GetCONFIG(CONFIGURATION, PKG_PATH): + + RESULT = {"Success": False, "ID": "", "Name": "", "urdf": "", "ee": ""} + + YAML_PATH = PKG_PATH + "/config/configurations.yaml" + + if not os.path.exists(YAML_PATH): + return (RESULT) + + with open(YAML_PATH, 'r') as YAML: + cYAML = yaml.safe_load(YAML) + + for x in cYAML["Configurations"]: + + if x["ID"] == CONFIGURATION: + RESULT["Success"] = True + RESULT["ID"] = x["ID"] + RESULT["Name"] = x["Name"] + RESULT["urdf"] = x["urdf"] + RESULT["rob"] = x["rob"] + RESULT["ee"] = x["ee"] + + return(RESULT) + +# GET EE-Controllers LIST: +def GetEEctr(EEName): + + RESULT = [] + + PATH = os.path.join(get_package_share_directory('ros2srrc_endeffectors'), EEName, 'config') + YAML_PATH = PATH + "/controller_moveit2.yaml" + + with open(YAML_PATH, 'r') as YAML: + cYAML = yaml.safe_load(YAML) + + for x in cYAML["controller_names"]: + RESULT.append(x) + + return(RESULT) + +# CHECK if CONTROLLER file exists for EE: + +def EEctrlEXISTS(EEName): + + PATH = os.path.join(get_package_share_directory('ros2srrc_endeffectors'), EEName, 'config') + YAML_PATH = PATH + "/controller.yaml" + + RES = os.path.exists(YAML_PATH) + return(RES) +def setup_gazebo_environment(): + """Setup Gazebo environment variables for both local and Docker""" + + # Always disable online model database (fixes hanging issue) + env_vars = [ + SetEnvironmentVariable(name='GAZEBO_MODEL_DATABASE_URI', value='') + ] + + # Detect environment and set appropriate model path + if os.path.exists('/dev_ws'): + # Docker environment + model_path = '/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/packages/ur5/ros2srrc_ur5_gazebo/models' + else: + # Local environment + model_path = '/home/shu/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/packages/ur5/ros2srrc_ur5_gazebo/models' + + # Get existing GAZEBO_MODEL_PATH and append our path + existing_path = os.environ.get('GAZEBO_MODEL_PATH', '') + if existing_path: + full_path = f"{model_path}:{existing_path}" + else: + full_path = model_path + + env_vars.append( + SetEnvironmentVariable(name='GAZEBO_MODEL_PATH', value=full_path) + ) + + return env_vars +# ========== **GENERATE LAUNCH DESCRIPTION** ========== # +def generate_launch_description(): + + LD = LaunchDescription() + + gazebo_env_vars = setup_gazebo_environment() + for env_var in gazebo_env_vars: + LD.add_action(env_var) + + # === INPUT ARGUMENT: ROS 2 PACKAGE === # + + #PACKAGE_NAME = AssignArgument("package") + # DMM: fixed UR5 + PACKAGE_NAME = "ros2srrc_ur5" + + if PACKAGE_NAME != None: + None + else: + print("") + print("ERROR: package INPUT ARGUMENT has not been defined. Please try again.") + print("Closing... BYE!") + exit() + + # CHECK if -> PACKAGE EXISTS, and GET PATH: + try: + PKG_PATH = get_package_share_directory(PACKAGE_NAME + "_gazebo") + except PackageNotFoundError: + print("") + print("ERROR: The defined ROS 2 Package was not found. Please try again.") + print("Closing... BYE!") + exit() + except ValueError: + print("") + print("ERROR: The defined ROS 2 Package name is not valid. Please try again.") + print("Closing... BYE!") + exit() + + # === INPUT ARGUMENT: CONFIGURATION === # + # CONFIG = AssignArgument("config") + # DMM fixed UR5 + Robotiq 85 + CONFIG = "ur5_7" + CONFIGURATION = GetCONFIG(CONFIG, PKG_PATH) + + if CONFIGURATION["Success"] == False: + print("") + print("ERROR: config INPUT ARGUMENT has not been correctly defined. Please try again.") + print("Closing... BYE!") + exit() + + # === INPUT ARGUMENT: HMI === # + HMI = AssignArgument("hmi") + if HMI == "True" or HMI == "true": + HMI = "true" + else: + HMI = "false" + + # ========== CELL INFORMATION ========== # + print("") + print("===== GAZEBO: Robot Simulation + MoveIt!2 Framework (" + PACKAGE_NAME + "_moveit2) =====") + print("Robot configuration:") + print(CONFIGURATION["ID"] + " -> " + CONFIGURATION["Name"]) + print("") + + # ***** GAZEBO ***** # + # DECLARE Gazebo WORLD file: + robot_gazebo = os.path.join( + get_package_share_directory(PACKAGE_NAME + '_gazebo'), + 'worlds', + PACKAGE_NAME +'_machine_vision'+ '.world') + # DECLARE Gazebo LAUNCH file: + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + launch_arguments={'world': robot_gazebo}.items(), + ) + + # ***** ROBOT DESCRIPTION ***** # + # Robot Description file package: + robot_description_path = os.path.join(get_package_share_directory(PACKAGE_NAME + '_gazebo')) + # ROBOT urdf file path: + xacro_file = os.path.join(robot_description_path,'urdf',CONFIGURATION["urdf"]) + # Generate ROBOT_DESCRIPTION variable: + doc = xacro.parse(open(xacro_file)) + + if CONFIGURATION["ee"] == "none": + EE = "false" + else: + EE = "true" + + xacro.process_doc(doc, mappings={ + "EE": EE, + "EE_name": CONFIGURATION["ee"], + "hmi": HMI, + }) + + # EE -> Controller file needed? + if EE == "true": + if EEctrlEXISTS(CONFIGURATION["ee"]) == False: + EE = "true-NOctr" + + robot_description_config = doc.toxml() + robot_description = {'robot_description': robot_description_config} + + # ROBOT STATE PUBLISHER NODE: + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[ + robot_description, + {"use_sim_time": True} + ] + ) + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], + ) + + # SPAWN ROBOT TO GAZEBO: + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', '-entity', CONFIGURATION["rob"]], + output='both') + + # ***** CONTROLLERS ***** # + # Joint STATE BROADCASTER: + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + # Joint TRAJECTORY Controller: + joint_trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], + ) + + # EE CONTROLLERS: + if EE == "true": + CONTROLLERS = GetEEctr(CONFIGURATION["ee"]) + CONTROLLER_NODES = [] + + for x in CONTROLLERS: + CONTROLLER_NODES.append( + Node( + package="controller_manager", + executable="spawner", + arguments=[x, "-c", "/controller_manager"], + ) + ) + + # *********************** MoveIt!2 *********************** # + + # Determine SRDF file path + if (EE == "false"): + srdf_file_path = "config/" + CONFIGURATION["rob"] + ".srdf" + else: + srdf_file_path = "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + ".srdf" + + # Build MoveIt configuration using MoveItConfigsBuilder + moveit_config_builder = MoveItConfigsBuilder(PACKAGE_NAME + "_moveit2") + + # Configure robot description semantic (SRDF) + moveit_config_builder = moveit_config_builder.robot_description_semantic( + file_path=srdf_file_path + ) + + # Configure kinematics + moveit_config_builder = moveit_config_builder.robot_description_kinematics( + file_path=os.path.join( + get_package_share_directory("ros2srrc_robots"), + CONFIGURATION["rob"], "config", "kinematics.yaml" + ) + ) + + # Configure joint limits + if (EE == "false") or (EE == "true-NOctr"): + joint_limits_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml") + joint_limits_file = os.path.join( + get_package_share_directory("ros2srrc_robots"), + CONFIGURATION["rob"], "config", "joint_limits.yaml" + ) + else: + # Properly merge robot and end-effector joint limits + YAML_ROB = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml")["joint_limits"] + YAML_EE = load_yaml("ros2srrc_endeffectors", CONFIGURATION["ee"] + "/config/joint_limits.yaml")["joint_limits"] + joint_limits_yaml = {} + joint_limits_yaml["joint_limits"] = YAML_ROB | YAML_EE + # For MoveItConfigsBuilder, we still use robot file (builder will be overridden by merged limits) + joint_limits_file = os.path.join( + get_package_share_directory("ros2srrc_robots"), + CONFIGURATION["rob"], "config", "joint_limits.yaml" + ) + + joint_limits = {'robot_description_planning': joint_limits_yaml} + + # Configure MoveItConfigsBuilder with robot joint limits file + moveit_config_builder = moveit_config_builder.joint_limits( + file_path=joint_limits_file + ) + + # Configure planning scene monitor + moveit_config_builder = moveit_config_builder.planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + publish_geometry_updates=True, + publish_state_updates=True, + publish_transforms_updates=True + ) + + # Configure trajectory execution - using robot controller config file + controller_config_path = os.path.join( + get_package_share_directory("ros2srrc_robots"), + CONFIGURATION["rob"], "config", "controller_moveit2.yaml" + ) + moveit_config_builder = moveit_config_builder.trajectory_execution( + file_path=controller_config_path + ) + + # Configure planning pipelines + moveit_config_builder = moveit_config_builder.planning_pipelines( + pipelines=["pilz_industrial_motion_planner"] + ) + + # Configure Pilz cartesian limits + moveit_config_builder = moveit_config_builder.pilz_cartesian_limits( + file_path=os.path.join( + get_package_share_directory("ros2srrc_robots"), + CONFIGURATION["rob"], "config", "pilz_cartesian_limits.yaml" + ) + ) + + # Add sensor configuration if available + sensors_3d_file = os.path.join( + get_package_share_directory(PACKAGE_NAME + "_moveit2"), + "config", "sensors_3d.yaml" + ) + if os.path.exists(sensors_3d_file): + moveit_config_builder = moveit_config_builder.sensors_3d( + file_path=sensors_3d_file + ) + + # Build the configuration + moveit_config = moveit_config_builder.to_moveit_configs() + + # Load additional configurations that need merging for EE + kinematics_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/kinematics.yaml") + pilz_cartesian_limits_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/pilz_cartesian_limits.yaml") + + # Add Pilz planner configuration + pilz_planning_pipeline_config = { + "move_group": { + "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", + "request_adapters": "", + "start_state_max_bounds_error": 0.1, + "default_planner_config": "PTP", + } + } + + # Add controller configuration for MoveIt + if (EE == "false") or (EE == "true-NOctr"): + moveit_simple_controllers_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml") + else: + # Properly merge robot and end-effector controller configs + YAML_ROB = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml") + YAML_EE = load_yaml("ros2srrc_endeffectors", CONFIGURATION["ee"] + "/config/controller_moveit2.yaml") + for x in YAML_ROB["controller_names"]: + YAML_EE["controller_names"].append(x) + moveit_simple_controllers_yaml = YAML_ROB | YAML_EE + + # Use the same structure as the backup file + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + # Add trajectory execution parameters from backup + 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, + } + + # Add planning scene monitor parameters from backup + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # CRITICAL: Add MoveGroup capabilities for gripper planning + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + + # MoveGroup Node - keep original planning scene structure + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, # Add the robot description from earlier + moveit_config.to_dict(), + pilz_planning_pipeline_config, + joint_limits, # This is critical - contains merged joint limits with acceleration limits for gripper + moveit_controllers, + { + "use_sim_time": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + "moveit_manage_controllers": True, + "planning_pipelines": ["move_group"], + # "default_planning_pipeline": "move_group", + "default_planning_pipeline": "move_group", + + "publish_geometry_updates": False, + "publish_planning_scene": False, + + # Octomap parameters + "octomap_resolution": 0.02, # Default is 0.1, smaller = finer resolution + "octomap_frame": "world", + "max_range": 5.0, + }, + ], + arguments=["--ros-args", "--log-level", "info"], + ) + + # RVIZ with all parameters from backup: + rviz_base = os.path.join(get_package_share_directory(PACKAGE_NAME + "_moveit2"), "config") + rviz_full_config = os.path.join(rviz_base + "/ur5robotiq_2f85_with_cam_moveit2.rviz") + + rviz_node_full = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_full_config], + parameters=[ + robot_description, + moveit_config.to_dict(), + joint_limits, # Add merged joint limits here too + {"use_sim_time": True}, + ] + ) + + # =================================================================================================== # + # ============================= ros2srrc_execution -> CUSTOM INTERFACES ============================= # + + # Move and Sequence: + if EE == "true": + + MoveInterface = Node( + name="move", + package="ros2srrc_execution", + executable="move", + output="screen", + parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": CONFIGURATION["ee"]}, {"ENV_PARAM": "gazebo"}], + ) + + else: + + MoveInterface = Node( + name="move", + package="ros2srrc_execution", + executable="move", + output="screen", + parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": "none"}, {"ENV_PARAM": "gazebo"}], + ) + + # RobMove and RobPose: + RobMoveInterface = Node( + name="robmove", + package="ros2srrc_execution", + executable="robmove", + output="screen", + parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], + ) + RobPoseInterface = Node( + name="robpose", + package="ros2srrc_execution", + executable="robpose", + output="screen", + parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], + ) + + + pcl_filter_node = Node( + package='pcl_filter', + executable='pcl_filter_server', + name='pcl_filter_server', + output='screen', + parameters=[{"use_sim_time": True}] # Add this to sync with simulation time + ) + + # =============================================== # + # ========== RETURN LAUNCH DESCRIPTION ========== # + + # Add ROS 2 Nodes to LaunchDescription() element: + LD.add_action(gazebo) + LD.add_action(node_robot_state_publisher) + LD.add_action(static_tf) + LD.add_action(spawn_entity) + + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = spawn_entity, + on_exit = [ + joint_state_broadcaster_spawner, + ] + ) + ) + ) + + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = spawn_entity, + on_exit = [ + joint_trajectory_controller_spawner, + ] + ) + ) + ) + + if EE == "true": + + for x in CONTROLLER_NODES: + + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = joint_trajectory_controller_spawner, + on_exit = [ + x, + ] + ) + ) + ) + + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = spawn_entity, + on_exit = [ + + # MoveIt!2: + TimerAction( + period=2.0, + actions=[ + rviz_node_full, + run_move_group_node, + ] + ), + + ] + ) + ) + ) + + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = spawn_entity, + on_exit = [ + + # Interfaces: + TimerAction( + period=5.0, + actions=[ + MoveInterface, + RobMoveInterface, + RobPoseInterface, + pcl_filter_node, + ] + ), + + ] + ) + ) + ) + + # ***** RETURN ***** # + return(LD) \ No newline at end of file From 87cdabf990606857c4f19d22e5a7872c69dca556 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 12:38:50 +0200 Subject: [PATCH 02/31] Try to fix spawn robot pose --- .../config/ur5robotiq_2f85_classic.srdf | 206 ++++++++++++++++++ .../machine_vision.launch.py | 6 +- database/universes.sql | 2 +- 3 files changed, 211 insertions(+), 3 deletions(-) create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85_classic.srdf diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85_classic.srdf new file mode 100644 index 000000000..f57d134ce --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85_classic.srdf @@ -0,0 +1,206 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 91e37dda4..66b018c40 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -311,10 +311,12 @@ def generate_launch_description(): if (EE == "false"): srdf_file_path = "config/" + CONFIGURATION["rob"] + ".srdf" else: - srdf_file_path = "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + ".srdf" + srdf_file_path = "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + "_classic.srdf" # Build MoveIt configuration using MoveItConfigsBuilder - moveit_config_builder = MoveItConfigsBuilder(PACKAGE_NAME + "_moveit2") + moveit_config_builder = MoveItConfigsBuilder(PACKAGE_NAME + "_moveit2").robot_description( + robot_description + ) # Configure robot description semantic (SRDF) moveit_config_builder = moveit_config_builder.robot_description_semantic( diff --git a/database/universes.sql b/database/universes.sql index 6e0850b40..a71210504 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -239,7 +239,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 35 Car Junction /opt/jderobot/Launchers/car_junction.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/car_junction.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 36 Drone Gymkhana Harmonic /opt/jderobot/Launchers/drone_gymkhana.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/drone_gymkhana.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 37 Tower Inspection Harmonic /opt/jderobot/Launchers/power_tower_inspection.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/power_tower_inspection.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} -38 Machine Vision Industrial /home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/machine_vision.launch.py {"rviz":"/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/machine_vision_rviz.launch.py"} ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} +38 Machine Vision Industrial /opt/jderobot/Launchers/machine_vision_classic/machine_vision_classic.launch.py {"rviz":"/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/machine_vision_rviz.launch.py"} ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 39 Labyrinth Escape /opt/jderobot/Launchers/labyrinth_escape.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/labyrinth_escape.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 40 Obstacle Avoidance Harmonic /opt/jderobot/Launchers/obstacle_avoidance_h.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/obstacle_avoidance.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 41 Autopark_line /opt/jderobot/Launchers/autopark_line.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} From b22bdf788e9b933964c2aac71185ed39246e0c3e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 13:39:59 +0200 Subject: [PATCH 03/31] Try to fix spawn robot pose --- database/universes.sql | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/database/universes.sql b/database/universes.sql index a71210504..661ff31ca 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -239,7 +239,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 35 Car Junction /opt/jderobot/Launchers/car_junction.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/car_junction.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 36 Drone Gymkhana Harmonic /opt/jderobot/Launchers/drone_gymkhana.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/drone_gymkhana.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 37 Tower Inspection Harmonic /opt/jderobot/Launchers/power_tower_inspection.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/power_tower_inspection.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} -38 Machine Vision Industrial /opt/jderobot/Launchers/machine_vision_classic/machine_vision_classic.launch.py {"rviz":"/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/machine_vision_rviz.launch.py"} ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} +38 Machine Vision Industrial /opt/jderobot/Launchers/machine_vision_classic/machine_vision.launch.py {"rviz":"/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/machine_vision_rviz.launch.py"} ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 39 Labyrinth Escape /opt/jderobot/Launchers/labyrinth_escape.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/labyrinth_escape.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 40 Obstacle Avoidance Harmonic /opt/jderobot/Launchers/obstacle_avoidance_h.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/obstacle_avoidance.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 41 Autopark_line /opt/jderobot/Launchers/autopark_line.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} From 331c0eca5157af6127f789019ff6821d410233d1 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 16:39:57 +0200 Subject: [PATCH 04/31] Try to fix spawn robot pose --- Launchers/machine_vision_classic/machine_vision.launch.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 66b018c40..6e6c838cd 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -314,9 +314,7 @@ def generate_launch_description(): srdf_file_path = "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + "_classic.srdf" # Build MoveIt configuration using MoveItConfigsBuilder - moveit_config_builder = MoveItConfigsBuilder(PACKAGE_NAME + "_moveit2").robot_description( - robot_description - ) + moveit_config_builder = MoveItConfigsBuilder(PACKAGE_NAME + "_moveit2") # Configure robot description semantic (SRDF) moveit_config_builder = moveit_config_builder.robot_description_semantic( From 7fe297a8a6fd4afae06d3f94ad6146d4cfcdd81c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 17:03:00 +0200 Subject: [PATCH 05/31] Try to fix spawn robot pose --- .../config/ur5robotiq_2f85_classic.srdf | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Industrial/ros2_SimRealRobotControl_gz/packages/ur5/{ros2srrc_ur5_moveit2 => ros2srrc_ur5_moveit2_moveit_config}/config/ur5robotiq_2f85_classic.srdf (100%) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf similarity index 100% rename from Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85_classic.srdf rename to Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf From dd490b82e214b9686766cf8c5d0d1ada2d574597 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 17:31:54 +0200 Subject: [PATCH 06/31] Fix srdf and controller.yaml --- .../config/ur5robotiq_2f85_classic.srdf | 6 ++-- .../robotiq_2f85/config/controller.yaml | 34 ++++++++++++++++--- 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf index f57d134ce..784a6c51c 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf @@ -34,7 +34,7 @@ - + @@ -57,7 +57,7 @@ - + @@ -66,7 +66,7 @@ - + diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml index 56e9651b4..360051d68 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml @@ -1,10 +1,36 @@ controller_manager: ros__parameters: - update_rate: 250 - gripper_controller: + update_rate: 250 #Hz + + robotiq_controller_LKJ: + type: position_controllers/GripperActionController + robotiq_controller_RKJ: + type: position_controllers/GripperActionController + robotiq_controller_LIKJ: + type: position_controllers/GripperActionController + robotiq_controller_RIKJ: + type: position_controllers/GripperActionController + robotiq_controller_LFTJ: + type: position_controllers/GripperActionController + robotiq_controller_RFTJ: type: position_controllers/GripperActionController -gripper_controller: +robotiq_controller_LKJ: + ros__parameters: + joint: robotiq_85_left_knuckle_joint +robotiq_controller_RKJ: + ros__parameters: + joint: robotiq_85_right_knuckle_joint +robotiq_controller_LIKJ: + ros__parameters: + joint: robotiq_85_left_inner_knuckle_joint +robotiq_controller_RIKJ: + ros__parameters: + joint: robotiq_85_right_inner_knuckle_joint +robotiq_controller_LFTJ: + ros__parameters: + joint: robotiq_85_left_finger_tip_joint +robotiq_controller_RFTJ: ros__parameters: - joint: robotiq_85_left_knuckle_joint \ No newline at end of file + joint: robotiq_85_right_finger_tip_joint \ No newline at end of file From 539ba3658e2a7ff76a4769eef99c2580dc76e7ef Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 18:11:36 +0200 Subject: [PATCH 07/31] Try to fix spawn robot pose --- .../machine_vision.launch.py | 36 +++++++++++-------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 6e6c838cd..4ad4cb05d 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -321,14 +321,6 @@ def generate_launch_description(): file_path=srdf_file_path ) - # Configure kinematics - moveit_config_builder = moveit_config_builder.robot_description_kinematics( - file_path=os.path.join( - get_package_share_directory("ros2srrc_robots"), - CONFIGURATION["rob"], "config", "kinematics.yaml" - ) - ) - # Configure joint limits if (EE == "false") or (EE == "true-NOctr"): joint_limits_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml") @@ -400,7 +392,21 @@ def generate_launch_description(): moveit_config = moveit_config_builder.to_moveit_configs() # Load additional configurations that need merging for EE - kinematics_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/kinematics.yaml") + raw_kinematics = load_yaml( + "ros2srrc_robots", + CONFIGURATION["rob"] + "/config/kinematics.yaml" + ) + + # Si el yaml viene limpio + if "ur5_manipulator" in raw_kinematics: + kinematics_yaml = { + "robot_description_kinematics": raw_kinematics + } + else: + # Si viene con /**/ros__parameters + kinematics_yaml = { + "robot_description_kinematics": raw_kinematics["/**"]["ros__parameters"] + } pilz_cartesian_limits_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/pilz_cartesian_limits.yaml") # Add Pilz planner configuration @@ -462,6 +468,7 @@ def generate_launch_description(): pilz_planning_pipeline_config, joint_limits, # This is critical - contains merged joint limits with acceleration limits for gripper moveit_controllers, + kinematics_yaml, { "use_sim_time": True, "trajectory_execution.allowed_execution_duration_scaling": 1.2, @@ -498,6 +505,7 @@ def generate_launch_description(): robot_description, moveit_config.to_dict(), joint_limits, # Add merged joint limits here too + kinematics_yaml, {"use_sim_time": True}, ] ) @@ -513,7 +521,7 @@ def generate_launch_description(): package="ros2srrc_execution", executable="move", output="screen", - parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": CONFIGURATION["ee"]}, {"ENV_PARAM": "gazebo"}], + parameters=[robot_description, moveit_config.robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": CONFIGURATION["ee"]}, {"ENV_PARAM": "gazebo"}], ) else: @@ -523,7 +531,7 @@ def generate_launch_description(): package="ros2srrc_execution", executable="move", output="screen", - parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": "none"}, {"ENV_PARAM": "gazebo"}], + parameters=[robot_description, moveit_config.robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": "none"}, {"ENV_PARAM": "gazebo"}], ) # RobMove and RobPose: @@ -532,14 +540,14 @@ def generate_launch_description(): package="ros2srrc_execution", executable="robmove", output="screen", - parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], + parameters=[robot_description, moveit_config.robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], ) RobPoseInterface = Node( name="robpose", package="ros2srrc_execution", executable="robpose", output="screen", - parameters=[robot_description, moveit_config.robot_description_semantic, {"robot_description_kinematics": kinematics_yaml}, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], + parameters=[robot_description, moveit_config.robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], ) @@ -601,7 +609,7 @@ def generate_launch_description(): # MoveIt!2: TimerAction( - period=2.0, + period=5.0, actions=[ rviz_node_full, run_move_group_node, From 8c81a0f58abfab628bdb60f01986f31a8b2a6322 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 18:30:12 +0200 Subject: [PATCH 08/31] Try to fix spawn robot pose --- Launchers/machine_vision_classic/machine_vision.launch.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 4ad4cb05d..81becf8f7 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -387,14 +387,18 @@ def generate_launch_description(): moveit_config_builder = moveit_config_builder.sensors_3d( file_path=sensors_3d_file ) + + moveit_config_builder = moveit_config_builder.robot_description_kinematics( + file_path=None + ) # Build the configuration moveit_config = moveit_config_builder.to_moveit_configs() # Load additional configurations that need merging for EE raw_kinematics = load_yaml( - "ros2srrc_robots", - CONFIGURATION["rob"] + "/config/kinematics.yaml" + "ur5_gripper_moveit_config", + "config/kinematics.yaml" ) # Si el yaml viene limpio From b0d08f6b489af5d2b5010c4b492328f0de1413d7 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 18:53:51 +0200 Subject: [PATCH 09/31] Fix Launcher --- .../machine_vision_classic/machine_vision.launch.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 81becf8f7..11d2a5c22 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -387,14 +387,13 @@ def generate_launch_description(): moveit_config_builder = moveit_config_builder.sensors_3d( file_path=sensors_3d_file ) - - moveit_config_builder = moveit_config_builder.robot_description_kinematics( - file_path=None - ) # Build the configuration moveit_config = moveit_config_builder.to_moveit_configs() + moveit_config_dict = moveit_config.to_dict() + moveit_config_dict.pop("robot_description_kinematics", None) + # Load additional configurations that need merging for EE raw_kinematics = load_yaml( "ur5_gripper_moveit_config", @@ -468,7 +467,7 @@ def generate_launch_description(): output="screen", parameters=[ robot_description, # Add the robot description from earlier - moveit_config.to_dict(), + moveit_config_dict, pilz_planning_pipeline_config, joint_limits, # This is critical - contains merged joint limits with acceleration limits for gripper moveit_controllers, @@ -507,7 +506,7 @@ def generate_launch_description(): arguments=["-d", rviz_full_config], parameters=[ robot_description, - moveit_config.to_dict(), + moveit_config_dict, joint_limits, # Add merged joint limits here too kinematics_yaml, {"use_sim_time": True}, From 53833f4e94e1e4b9ddb5d969aad4ed80494f7557 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 19:09:09 +0200 Subject: [PATCH 10/31] Add Kinematics.yaml --- .../config/kinematics.yaml | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..7e8f5427a --- /dev/null +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + ur5_manipulator: + 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 From f4280446cf14369bb2363c63ab79f2d9f0996869 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 19:27:38 +0200 Subject: [PATCH 11/31] Fix Kinematics.yaml --- .../ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml index 7e8f5427a..11a55712a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml @@ -5,5 +5,6 @@ kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 - gripper: + + robotiq_2f85: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin \ No newline at end of file From 2d575c634c4bcca2d6f2aab1ddf4beadf90a2ce5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 19:50:59 +0200 Subject: [PATCH 12/31] Fix srdf --- .../config/ur5robotiq_2f85_classic.srdf | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf index 784a6c51c..f57d134ce 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf @@ -34,7 +34,7 @@ - + @@ -57,7 +57,7 @@ - + @@ -66,7 +66,7 @@ - + From 2759d66aee3775157f0ccf997dd77767390c514e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 20:28:56 +0200 Subject: [PATCH 13/31] Fix kinematics.yaml --- .../ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml index 11a55712a..00dc4c294 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/kinematics.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - ur5_manipulator: + ur5_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 From 6249755109ea606b3b1fcbcb46426bfb3aba003c Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 2 May 2026 20:52:58 +0200 Subject: [PATCH 14/31] Fix srdf --- .../config/ur5robotiq_2f85_classic.srdf | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf index f57d134ce..784a6c51c 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf @@ -34,7 +34,7 @@ - + @@ -57,7 +57,7 @@ - + @@ -66,7 +66,7 @@ - + From b2e7e43256311777b43024d30cae0f646571bed0 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 10:59:12 +0200 Subject: [PATCH 15/31] Fix gripper movement --- .../machine_vision.launch.py | 71 ++++++------------- 1 file changed, 20 insertions(+), 51 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 11d2a5c22..6e7176ba9 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -291,19 +291,11 @@ def generate_launch_description(): arguments=["joint_trajectory_controller", "-c", "/controller_manager"], ) - # EE CONTROLLERS: - if EE == "true": - CONTROLLERS = GetEEctr(CONFIGURATION["ee"]) - CONTROLLER_NODES = [] - - for x in CONTROLLERS: - CONTROLLER_NODES.append( - Node( - package="controller_manager", - executable="spawner", - arguments=[x, "-c", "/controller_manager"], - ) - ) + gripper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["gripper_controller", "-c", "/controller_manager"], + ) # *********************** MoveIt!2 *********************** # @@ -355,15 +347,7 @@ def generate_launch_description(): publish_state_updates=True, publish_transforms_updates=True ) - - # Configure trajectory execution - using robot controller config file - controller_config_path = os.path.join( - get_package_share_directory("ros2srrc_robots"), - CONFIGURATION["rob"], "config", "controller_moveit2.yaml" - ) - moveit_config_builder = moveit_config_builder.trajectory_execution( - file_path=controller_config_path - ) + # Configure planning pipelines moveit_config_builder = moveit_config_builder.planning_pipelines( @@ -422,22 +406,12 @@ def generate_launch_description(): } } - # Add controller configuration for MoveIt - if (EE == "false") or (EE == "true-NOctr"): - moveit_simple_controllers_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml") - else: - # Properly merge robot and end-effector controller configs - YAML_ROB = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml") - YAML_EE = load_yaml("ros2srrc_endeffectors", CONFIGURATION["ee"] + "/config/controller_moveit2.yaml") - for x in YAML_ROB["controller_names"]: - YAML_EE["controller_names"].append(x) - moveit_simple_controllers_yaml = YAML_ROB | YAML_EE - - # Use the same structure as the backup file - moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", - } + moveit_controllers = load_yaml( + "ur5_gripper_moveit_config", + "config/moveit_controllers.yaml" + ) + + moveit_controllers = moveit_controllers["/**"]["ros__parameters"] # Add trajectory execution parameters from backup trajectory_execution = { @@ -591,19 +565,14 @@ def generate_launch_description(): ) ) - if EE == "true": - - for x in CONTROLLER_NODES: - - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = joint_trajectory_controller_spawner, - on_exit = [ - x, - ] - ) - ) - ) + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = joint_trajectory_controller_spawner, + on_exit = [ + gripper_controller_spawner, + ] + ) + )) LD.add_action(RegisterEventHandler( OnProcessExit( From 9fc1d8266cd9da7d6db84e59c7e8aaea3c8207ef Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 11:35:32 +0200 Subject: [PATCH 16/31] Fix controller.yaml --- .../robotiq_2f85/config/controller.yaml | 34 +++---------------- 1 file changed, 4 insertions(+), 30 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml index 360051d68..56e9651b4 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml @@ -1,36 +1,10 @@ controller_manager: ros__parameters: + update_rate: 250 - update_rate: 250 #Hz - - robotiq_controller_LKJ: - type: position_controllers/GripperActionController - robotiq_controller_RKJ: - type: position_controllers/GripperActionController - robotiq_controller_LIKJ: - type: position_controllers/GripperActionController - robotiq_controller_RIKJ: - type: position_controllers/GripperActionController - robotiq_controller_LFTJ: - type: position_controllers/GripperActionController - robotiq_controller_RFTJ: + gripper_controller: type: position_controllers/GripperActionController -robotiq_controller_LKJ: - ros__parameters: - joint: robotiq_85_left_knuckle_joint -robotiq_controller_RKJ: - ros__parameters: - joint: robotiq_85_right_knuckle_joint -robotiq_controller_LIKJ: - ros__parameters: - joint: robotiq_85_left_inner_knuckle_joint -robotiq_controller_RIKJ: - ros__parameters: - joint: robotiq_85_right_inner_knuckle_joint -robotiq_controller_LFTJ: - ros__parameters: - joint: robotiq_85_left_finger_tip_joint -robotiq_controller_RFTJ: +gripper_controller: ros__parameters: - joint: robotiq_85_right_finger_tip_joint \ No newline at end of file + joint: robotiq_85_left_knuckle_joint \ No newline at end of file From 46c0c34d5480c25e63af17e88df488c502fa9e08 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 12:08:31 +0200 Subject: [PATCH 17/31] Fix launcher --- .../machine_vision.launch.py | 28 ++++--------------- 1 file changed, 5 insertions(+), 23 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 6e7176ba9..248e3d9ce 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -545,34 +545,16 @@ def generate_launch_description(): LD.add_action(static_tf) LD.add_action(spawn_entity) - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ + LD.add_action( + TimerAction( + period=3.0, + actions=[ joint_state_broadcaster_spawner, - ] - ) - ) - ) - - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ joint_trajectory_controller_spawner, - ] - ) - ) - ) - - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = joint_trajectory_controller_spawner, - on_exit = [ gripper_controller_spawner, ] ) - )) + ) LD.add_action(RegisterEventHandler( OnProcessExit( From 6a48390c649a7f9ca1ad30f4b7babcafb3c0e30e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 16:32:43 +0200 Subject: [PATCH 18/31] Fix Launchers --- .../machine_vision.launch.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 248e3d9ce..e12f609d3 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -303,10 +303,14 @@ def generate_launch_description(): if (EE == "false"): srdf_file_path = "config/" + CONFIGURATION["rob"] + ".srdf" else: - srdf_file_path = "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + "_classic.srdf" + srdf_file_path = os.path.join( + get_package_share_directory("ros2srrc_ur5_moveit2_moveit_config"), + "config", + "ur5robotiq_2f85_classic.srdf" + ) # Build MoveIt configuration using MoveItConfigsBuilder - moveit_config_builder = MoveItConfigsBuilder(PACKAGE_NAME + "_moveit2") + moveit_config_builder = MoveItConfigsBuilder("ros2srrc_ur5_moveit2_moveit_config") # Configure robot description semantic (SRDF) moveit_config_builder = moveit_config_builder.robot_description_semantic( @@ -452,9 +456,8 @@ def generate_launch_description(): "trajectory_execution.allowed_goal_duration_margin": 0.5, "trajectory_execution.allowed_start_tolerance": 0.01, "moveit_manage_controllers": True, - "planning_pipelines": ["move_group"], - # "default_planning_pipeline": "move_group", - "default_planning_pipeline": "move_group", + "planning_pipelines": ["pilz_industrial_motion_planner"], + "default_planning_pipeline": "pilz_industrial_motion_planner", "publish_geometry_updates": False, "publish_planning_scene": False, @@ -582,7 +585,7 @@ def generate_launch_description(): # Interfaces: TimerAction( - period=5.0, + period=8.0, actions=[ MoveInterface, RobMoveInterface, From 8454bc18a3114891bc3b5bca37e915e14598d026 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 16:40:12 +0200 Subject: [PATCH 19/31] Fix kinematics and launcher --- .../config/kinematics_classic.yaml | 10 ++++++++++ .../machine_vision_classic/machine_vision.launch.py | 4 ++-- 2 files changed, 12 insertions(+), 2 deletions(-) create mode 100644 Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml diff --git a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml new file mode 100644 index 000000000..11a55712a --- /dev/null +++ b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + ur5_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + + robotiq_2f85: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin \ No newline at end of file diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index e12f609d3..9bad61244 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -310,7 +310,7 @@ def generate_launch_description(): ) # Build MoveIt configuration using MoveItConfigsBuilder - moveit_config_builder = MoveItConfigsBuilder("ros2srrc_ur5_moveit2_moveit_config") + moveit_config_builder = MoveItConfigsBuilder("ros2srrc_ur5_moveit2") # Configure robot description semantic (SRDF) moveit_config_builder = moveit_config_builder.robot_description_semantic( @@ -385,7 +385,7 @@ def generate_launch_description(): # Load additional configurations that need merging for EE raw_kinematics = load_yaml( "ur5_gripper_moveit_config", - "config/kinematics.yaml" + "config/kinematics_classic.yaml" ) # Si el yaml viene limpio From b5f2ee04ef838bfb0f9322b86b578d6a72e49f18 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 17:12:22 +0200 Subject: [PATCH 20/31] Fix kinematics.yaml --- .../config/kinematics_classic.yaml | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml index 11a55712a..8967bc79a 100644 --- a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml +++ b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml @@ -1,10 +1,5 @@ -/**: - ros__parameters: - ur5_manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 - - robotiq_2f85: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin \ No newline at end of file +ur5_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file From 9ef787af3abbbb2412ccfd801f13a3f4c68cc333 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 17:41:52 +0200 Subject: [PATCH 21/31] Change ur5_manipulator to ur5_arm --- .../config/ur5robotiq_2f85_classic.srdf | 6 +++--- .../config/kinematics_classic.yaml | 2 +- Launchers/machine_vision_classic/machine_vision.launch.py | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf index 784a6c51c..f57d134ce 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf @@ -34,7 +34,7 @@ - + @@ -57,7 +57,7 @@ - + @@ -66,7 +66,7 @@ - + diff --git a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml index 8967bc79a..38692e383 100644 --- a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml +++ b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml @@ -1,4 +1,4 @@ -ur5_manipulator: +ur5_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 9bad61244..6e87d7af9 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -389,7 +389,7 @@ def generate_launch_description(): ) # Si el yaml viene limpio - if "ur5_manipulator" in raw_kinematics: + if "ur5_arm" in raw_kinematics: kinematics_yaml = { "robot_description_kinematics": raw_kinematics } From 6abd89cd488e5f30d5c028085a28b4cd1783a4fa Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 18:13:17 +0200 Subject: [PATCH 22/31] Change again to ur5_manipulator --- .../config/ur5robotiq_2f85_classic.srdf | 6 +++--- .../config/kinematics_classic.yaml | 10 ++++++++-- .../machine_vision_classic/machine_vision.launch.py | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf index f57d134ce..784a6c51c 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf @@ -34,7 +34,7 @@ - + @@ -57,7 +57,7 @@ - + @@ -66,7 +66,7 @@ - + diff --git a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml index 38692e383..f445f1a65 100644 --- a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml +++ b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml @@ -1,5 +1,11 @@ -ur5_arm: +ur5_manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 \ No newline at end of file + kinematics_solver_attempts: 3 + +robotiq_2f85: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 1 \ No newline at end of file diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 6e87d7af9..9bad61244 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -389,7 +389,7 @@ def generate_launch_description(): ) # Si el yaml viene limpio - if "ur5_arm" in raw_kinematics: + if "ur5_manipulator" in raw_kinematics: kinematics_yaml = { "robot_description_kinematics": raw_kinematics } From 8300823c70eb90700ddefe1704bc4983308a7e71 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sun, 3 May 2026 18:31:04 +0200 Subject: [PATCH 23/31] Fix kinematics and srdf --- .../config/ur5robotiq_2f85_classic.srdf | 42 ++++--------------- .../config/kinematics_classic.yaml | 8 +--- 2 files changed, 9 insertions(+), 41 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf index 784a6c51c..17c87a7e5 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2_moveit_config/config/ur5robotiq_2f85_classic.srdf @@ -39,22 +39,8 @@ - - - - - - - - - - - - - - - - + + @@ -76,30 +62,18 @@ - - - - - - + + - - - - - - + + - - - - - - + + diff --git a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml index f445f1a65..8967bc79a 100644 --- a/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml +++ b/Industrial/ur5_gripper_moveit_config/config/kinematics_classic.yaml @@ -2,10 +2,4 @@ ur5_manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 - -robotiq_2f85: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 1 \ No newline at end of file + kinematics_solver_attempts: 3 \ No newline at end of file From 7ff49a97d16376d17134611b2002a5d853ea4be2 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 13:07:35 +0200 Subject: [PATCH 24/31] Fix move.cpp --- .../ros2srrc_execution/src/move.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp index 4e72de49a..c4bf5e302 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp @@ -401,10 +401,13 @@ class ActionServer : public rclcpp::Node if (RES == "PLANNING: OK"){ robot_trajectory::RobotTrajectory rt( - move_group_interface_ROB.getRobotModel(), - "ur5_manipulator" + (action == "MoveG" ? move_group_interface_EE.getRobotModel() : move_group_interface_ROB.getRobotModel()), + (action == "MoveG" ? param_EE : "ur5_manipulator") ); + moveit::core::RobotStatePtr current_state = + (action == "MoveG" ? move_group_interface_EE.getCurrentState() : move_group_interface_ROB.getCurrentState()); + moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); rt.setRobotTrajectoryMsg( @@ -421,7 +424,13 @@ class ActionServer : public rclcpp::Node rt.getRobotTrajectoryMsg(MyPlan.trajectory_); - bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + bool ExecSUCCESS; + + if (action == "MoveG"){ + ExecSUCCESS = (move_group_interface_EE.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + } else { + ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + } if (goal_handle->is_canceling()) { RCLCPP_INFO(this->get_logger(), "Goal canceled."); From 4cdde9f7ea6d5e92b99d93765bb57dffec745027 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 13:29:56 +0200 Subject: [PATCH 25/31] Fix move.cpp --- .../ros2srrc_execution/src/move.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp index c4bf5e302..d5b0c9fb3 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp @@ -408,8 +408,6 @@ class ActionServer : public rclcpp::Node moveit::core::RobotStatePtr current_state = (action == "MoveG" ? move_group_interface_EE.getCurrentState() : move_group_interface_ROB.getCurrentState()); - moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); - rt.setRobotTrajectoryMsg( *current_state, MyPlan.trajectory_ @@ -440,7 +438,8 @@ class ActionServer : public rclcpp::Node } if (ExecSUCCESS){ - RCLCPP_INFO(this->get_logger(), "%s - %s: Movement executed!", param_ROB.c_str(), action.c_str()); + std::string name = (action == "MoveG") ? param_EE : param_ROB; + RCLCPP_INFO(this->get_logger(), "%s - %s: Movement executed!", name.c_str(), action.c_str()); result->result = action + ":SUCCESS"; goal_handle->succeed(result); } else { From 6143cf204787af44fa68d2d67b731838e9b19fef Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 13:47:44 +0200 Subject: [PATCH 26/31] Restart move.cpp --- .../ros2srrc_execution/src/move.cpp | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp index d5b0c9fb3..4e72de49a 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp @@ -401,12 +401,11 @@ class ActionServer : public rclcpp::Node if (RES == "PLANNING: OK"){ robot_trajectory::RobotTrajectory rt( - (action == "MoveG" ? move_group_interface_EE.getRobotModel() : move_group_interface_ROB.getRobotModel()), - (action == "MoveG" ? param_EE : "ur5_manipulator") + move_group_interface_ROB.getRobotModel(), + "ur5_manipulator" ); - moveit::core::RobotStatePtr current_state = - (action == "MoveG" ? move_group_interface_EE.getCurrentState() : move_group_interface_ROB.getCurrentState()); + moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState(); rt.setRobotTrajectoryMsg( *current_state, @@ -422,13 +421,7 @@ class ActionServer : public rclcpp::Node rt.getRobotTrajectoryMsg(MyPlan.trajectory_); - bool ExecSUCCESS; - - if (action == "MoveG"){ - ExecSUCCESS = (move_group_interface_EE.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - } else { - ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - } + bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); if (goal_handle->is_canceling()) { RCLCPP_INFO(this->get_logger(), "Goal canceled."); @@ -438,8 +431,7 @@ class ActionServer : public rclcpp::Node } if (ExecSUCCESS){ - std::string name = (action == "MoveG") ? param_EE : param_ROB; - RCLCPP_INFO(this->get_logger(), "%s - %s: Movement executed!", name.c_str(), action.c_str()); + RCLCPP_INFO(this->get_logger(), "%s - %s: Movement executed!", param_ROB.c_str(), action.c_str()); result->result = action + ":SUCCESS"; goal_handle->succeed(result); } else { From eca58ad0f96a0aaaee46e7f3253fdc9a6ed47a01 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 16:58:10 +0200 Subject: [PATCH 27/31] Fix move.cpp --- .../ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp index 4e72de49a..6739b08a3 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp @@ -152,7 +152,7 @@ moveit::planning_interface::MoveGroupInterface::Plan plan_EE() { // Execute the plan if (success) { - RES = "PLANNING: OK"; + RES = "PLANNING: OK (EE)"; return(my_plan); } else From 78f074d42e261cc94f42a04a92ac75365b728010 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 17:29:26 +0200 Subject: [PATCH 28/31] Fix .yaml and moveg.cpp and change launcher --- .../robotiq_2f85/config/joint_limits.yaml | 32 ++++++------------- .../ros2srrc_execution/src/moveg.cpp | 18 ++++++++--- .../config/moveit_controllers_classic.yaml | 26 +++++++++++++++ .../machine_vision.launch.py | 2 +- 4 files changed, 51 insertions(+), 27 deletions(-) create mode 100644 Industrial/ur5_gripper_moveit_config/config/moveit_controllers_classic.yaml diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/joint_limits.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/joint_limits.yaml index 6a7f7a2da..e7c55e77f 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/joint_limits.yaml +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/joint_limits.yaml @@ -1,31 +1,19 @@ joint_limits: + robotiq_85_left_knuckle_joint: + has_position_limits: true + min_position: 0.0 + max_position: 0.8 has_velocity_limits: true max_velocity: 0.5 has_acceleration_limits: true - max_acceleration: 1.00 + max_acceleration: 1.0 + robotiq_85_right_knuckle_joint: + has_position_limits: true + min_position: 0.0 + max_position: 0.8 has_velocity_limits: true max_velocity: 0.5 has_acceleration_limits: true - max_acceleration: 1.00 - robotiq_85_left_inner_knuckle_joint: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 1.00 - robotiq_85_right_inner_knuckle_joint: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 1.00 - robotiq_85_left_finger_tip_joint: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 1.00 - robotiq_85_right_finger_tip_joint: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 1.00 + max_acceleration: 1.0 \ No newline at end of file diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/moveg.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/moveg.cpp index 4551f4ab2..27b2c3ee9 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/moveg.cpp +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/moveg.cpp @@ -69,12 +69,22 @@ MoveGSTRUCT MoveGAction (double VAL, std::vector JP, ros2srrc_data::msg: }; // 2. CONVERT -> VAL to GripperPose value (GP): - double GP = (GPMax - GPMin) * (VAL/100.0); + double GP = GPMin + (GPMax - GPMin) * (VAL / 100.0); // 3. SET GRIPPER POSE vector: - for (int i=0; i GPMax) value = GPMax; + if (value < GPMin) value = GPMin; + + JP[i] = value; + } RESULT.RES = "LIMITS: OK"; RESULT.JP = JP; diff --git a/Industrial/ur5_gripper_moveit_config/config/moveit_controllers_classic.yaml b/Industrial/ur5_gripper_moveit_config/config/moveit_controllers_classic.yaml new file mode 100644 index 000000000..8858789d1 --- /dev/null +++ b/Industrial/ur5_gripper_moveit_config/config/moveit_controllers_classic.yaml @@ -0,0 +1,26 @@ +/**: + 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: false + joints: + - robotiq_85_left_knuckle_joint + - robotiq_85_right_knuckle_joint \ No newline at end of file diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 9bad61244..8b987d783 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -412,7 +412,7 @@ def generate_launch_description(): moveit_controllers = load_yaml( "ur5_gripper_moveit_config", - "config/moveit_controllers.yaml" + "config/moveit_controllers_classic.yaml" ) moveit_controllers = moveit_controllers["/**"]["ros__parameters"] From f079a0cd34a85416a4ab66952758167c58c8b7c1 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 18:04:09 +0200 Subject: [PATCH 29/31] Fix gripper movement --- .../urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro | 8 +------- Launchers/machine_vision_classic/machine_vision.launch.py | 8 ++++++++ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro index 6d2443e2e..c10b1cf17 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85_with_cams_classic.urdf.xacro @@ -93,13 +93,7 @@ gazebo_ros2_control/GazeboSystem - - $(find ros2srrc_robots)/ur5/config/controller.yaml - - - - $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml - + $(find ur5_gripper_description)/config/ur5_controllers.yaml diff --git a/Launchers/machine_vision_classic/machine_vision.launch.py b/Launchers/machine_vision_classic/machine_vision.launch.py index 8b987d783..55a8e0557 100644 --- a/Launchers/machine_vision_classic/machine_vision.launch.py +++ b/Launchers/machine_vision_classic/machine_vision.launch.py @@ -232,6 +232,13 @@ def generate_launch_description(): robot_description_path = os.path.join(get_package_share_directory(PACKAGE_NAME + '_gazebo')) # ROBOT urdf file path: xacro_file = os.path.join(robot_description_path,'urdf',CONFIGURATION["urdf"]) + + controllers_file = os.path.join( + get_package_share_directory("ur5_gripper_description"), + "config", + "ur5_controllers.yaml" + ) + # Generate ROBOT_DESCRIPTION variable: doc = xacro.parse(open(xacro_file)) @@ -244,6 +251,7 @@ def generate_launch_description(): "EE": EE, "EE_name": CONFIGURATION["ee"], "hmi": HMI, + "simulation_controllers": controllers_file, }) # EE -> Controller file needed? From 1c7874b4c3a507ccdbf8d6f59c5321e690308f37 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 18:48:00 +0200 Subject: [PATCH 30/31] Fix gripper movement --- .../ur5/urdf/ur5_macro_classic.urdf.xacro | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro index 670b09be9..ff3fe5db8 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro @@ -76,19 +76,6 @@ - - - - From e92bf5c444493c9a103dd3935dbde95e3bdfbf9e Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 5 May 2026 19:05:06 +0200 Subject: [PATCH 31/31] Fix gripper movement --- .../ur5/urdf/ur5_macro_classic.urdf.xacro | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro index ff3fe5db8..670b09be9 100644 --- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro +++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_macro_classic.urdf.xacro @@ -76,6 +76,19 @@ + + + +