change pick_place_harmonic.launch.py

Author: Jepear19

Industrial/ur5_gripper_moveit_config/srdf/ur_gripper_macro.srdf.xacro

@@ -60,6 +60,13 @@
     <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
     <end_effector name="gripper" parent_link="${prefix}tool0" group="gripper" parent_group="${prefix}${name}_manipulator" />
 
+    <!-- PASSIVE JOINTS (FIX para mimic joints) -->
+    <passive_joint name="robotiq_85_right_knuckle_joint"/>
+    <passive_joint name="robotiq_85_left_inner_knuckle_joint"/>
+    <passive_joint name="robotiq_85_right_inner_knuckle_joint"/>
+    <passive_joint name="robotiq_85_left_finger_tip_joint"/>
+    <passive_joint name="robotiq_85_right_finger_tip_joint"/>
+
     <!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <!--PASSIVE JOINTS - MoveIt automatically detects mimic joints from URDF, don't declare them here -->
     <!-- Mimic joints (robotiq_85_right_knuckle_joint, robotiq_85_left_inner_knuckle_joint, etc.) 

Launchers/pick_place_harmonic/pick_place_harmonic.launch.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Pick Place Harmonic - FIXED (with correct XACRO → URDF)
+Pick Place Harmonic - CLASSIC MODE (NO MoveIt)
 """
 
 import os
@@ -50,7 +50,7 @@ def generate_launch_description():
     )
 
     # =========================
-    # XACRO
+    # XACRO → URDF
     # =========================
 
     xacro_file = "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro"
@@ -65,7 +65,7 @@ def generate_launch_description():
     }
 
     # =========================
-    # SRDF + CONFIG
+    # CONFIG (necesario para execution)
     # =========================
 
     robot_description_semantic = {
@@ -82,57 +82,8 @@ def generate_launch_description():
         )
     }
 
-    joint_limits = {
-        "robot_description_planning": load_yaml(
-            "ros2srrc_robots",
-            "ur5/config/joint_limits.yaml"
-        )
-    }
-
-    moveit_controllers = {
-        "moveit_simple_controller_manager": load_yaml(
-            "ros2srrc_robots",
-            "ur5/config/controller_moveit2.yaml"
-        ),
-        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
-    }
-
-    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,
-    }
-
-    planning_scene_monitor_parameters = {
-        "publish_planning_scene": True,
-        "publish_geometry_updates": True,
-        "publish_state_updates": True,
-        "publish_transforms_updates": True,
-    }
-
-    # =========================
-    # MOVE GROUP
-    # =========================
-
-    move_group_node = Node(
-        package="moveit_ros_move_group",
-        executable="move_group",
-        output="screen",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            kinematics_yaml,
-            joint_limits,
-            moveit_controllers,
-            trajectory_execution,
-            planning_scene_monitor_parameters,
-            {"use_sim_time": True},
-        ],
-    )
-
     # =========================
-    # EXECUTION NODES
+    # EXECUTION NODES (CLAVE)
     # =========================
 
     robmove_node = Node(
@@ -176,17 +127,17 @@ def generate_launch_description():
         ],
     )
 
+    # =========================
+    # LAUNCH
+    # =========================
+
     return LaunchDescription([
         world_launch,
         robot_launch,
 
+        # Espera corta para que Gazebo arranque
         TimerAction(
-            period=3.0,
-            actions=[move_group_node]
-        ),
-
-        TimerAction(
-            period=10.0,
+            period=2.0,
             actions=[robmove_node, robpose_node, move_node]
         )
     ])

Launchers/pick_place_harmonic/robot.launch.py

@@ -1,65 +1,35 @@
-#!/usr/bin/env python3
+"""
+Pick Place Harmonic - Robot Launcher
+Wraps spawn_robot_warehouse.launch.py
+"""
 
 import os
-import xacro
-
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
 
-    pkg = get_package_share_directory("ros2srrc_ur5_gazebo")
-
-    xacro_file = os.path.join(pkg, "urdf", "ur5_robotiq_2f85.urdf.xacro")
-
-    doc = xacro.parse(open(xacro_file))
-    xacro.process_doc(doc, mappings={
-        "EE": "true",
-        "EE_name": "robotiq_2f85"
-    })
+    try:
+        ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description")
+    except Exception as e:
+        print(f"ERROR: Cannot find ur5_gripper_description package: {e}")
+        print("Make sure packages are built in /home/ws")
+        raise
 
-    robot_description = {
-        "robot_description": doc.toxml()
-    }
-
-    robot_state_publisher = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="screen",
-        parameters=[robot_description, {"use_sim_time": True}],
+    warehouse_launch_file = os.path.join(
+        ur5_gripper_pkg, "launch", "spawn_robot_warehouse.launch.py"
     )
 
-    spawn_entity = Node(
-        package="gazebo_ros",
-        executable="spawn_entity.py",
-        arguments=["-topic", "robot_description", "-entity", "ur5"],
-        output="screen",
-    )
-
-    joint_state_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
-    )
-
-    joint_trajectory_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
-    )
+    print(f"Including launch file: {warehouse_launch_file}")
 
-    gripper_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["gripper_controller", "-c", "/controller_manager"],
+    warehouse_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(warehouse_launch_file),
+        launch_arguments={
+            "launch_rviz": "false",
+        }.items(),
     )
 
-    return LaunchDescription([
-        robot_state_publisher,
-        spawn_entity,
-        joint_state_broadcaster,
-        joint_trajectory_controller,
-        gripper_controller,
-    ])
+    return LaunchDescription([warehouse_launch])