diff --git a/CMakeLists.txt b/CMakeLists.txt index 2cdab50..867b229 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(multi_robot_arm) # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(gazebo_ros REQUIRED) +find_package(ros_gz_sim REQUIRED) include_directories(include) link_directories( @@ -16,7 +16,14 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS std_msgs ) -install(DIRECTORY launch config urdf worlds DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch config urdf worlds scripts DESTINATION share/${PROJECT_NAME}) + +# Install Python scripts +install(PROGRAMS + scripts/dance_arms.py + scripts/activate_controllers.py + DESTINATION lib/${PROJECT_NAME} +) ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/multi_robot_arm.dsv.in") ament_package() diff --git a/README.md b/README.md index 1c546ae..4f73aee 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,26 @@ The UR5 robotic arm, a versatile and widely-used model in robotics, serves as th ![image](https://github.com/arshadlab/multi_robot_arm/assets/85929438/4e052e79-65c7-4fe5-b73a-871b76b9f01e) ![image](https://github.com/arshadlab/multi_robot_arm/assets/85929438/3189c420-33c1-424c-aaa4-0cc2b8cc868c) +## Demo: Gazebo Harmonic Simulation +Check out the demo video showcasing multi-robot arm simulation with Gazebo Harmonic: + +![Gazebo Harmonic Demo](./run.mp4) + +## What's New + +### ROS2 Jazzy & Gazebo Harmonic Support +We've added full support for **ROS2 Jazzy** and **Gazebo Harmonic**, the latest stable distributions in the ROS2 ecosystem. This brings several benefits: + +- **Enhanced Physics Simulation**: Gazebo Harmonic provides improved physics accuracy and performance +- **Improved Compatibility**: Full integration with modern ROS2 tooling and libraries +- **Better Performance**: Optimized for the latest hardware and software stacks +- **ROS-Gazebo Bridge**: Native support via `ros-gz` for seamless ROS2-Gazebo integration + +### Launch Files +- **`gazebo_arm.launch.py`**: For ROS2 Humble + Gazebo 11 (legacy) +- **`gazebo_harmonic_arm.launch.py`**: For ROS2 Jazzy + Gazebo Harmonic (recommended) + +## Python MoveIt2 Bindings The present version of this tutorial employs a fork of pymoveit2, a Python library developed to facilitate interaction with MoveIt2. This has been an effective approach and served the purpose well. However, in recent times, MoveIt2 has introduced native Python bindings in its codebase. These bindings enable direct interaction with MoveIt2 via Python, eliminating the need for additional libraries like pymoveit2. This advancement simplifies the setup process, reduces dependency issues, and potentially enhances performance and stability. @@ -13,13 +33,35 @@ However, in recent times, MoveIt2 has introduced native Python bindings in its c As part of ongoing improvements and in the spirit of keeping up with these updates, future versions of this tutorial may transition to using the Python bindings provided directly by MoveIt2. This would replace the current reliance on the pymoveit2 fork, streamlining the implementation process and ensuring compatibility with future developments in MoveIt2. Consequently, this change will not only refine the tutorial but also make it more adaptable and robust for future use-cases. ## Setup -The package is verified with ROS2 Foxy and Humble. -Dependencies: ROS2 [Foxy or Humble], moveit2, pymoveit2, Gazebo 11 +### Supported Configurations +The package is verified and tested with the following configurations: + +| ROS2 Version | Gazebo Version | Launch File | Status | +|---|---|---|---| +| Humble | 11 | `gazebo_arm.launch.py` | ✓ Tested | +| Jazzy | Harmonic | `gazebo_harmonic_arm.launch.py` | ✓ Tested | + +### Dependencies + +**For ROS2 Humble + Gazebo 11:** +- ROS2 Humble +- moveit2 +- pymoveit2 +- Gazebo 11 + +**For ROS2 Jazzy + Gazebo Harmonic:** +- ROS2 Jazzy +- moveit2 +- pymoveit2 +- Gazebo Harmonic +- ros-gz (ROS-Gazebo bridge) ## Clone and Build -``` -source /opt/ros//setup.bash + +### For ROS2 Humble +```bash +source /opt/ros/humble/setup.bash mkdir -p robot_ws/src cd robot_ws/src git clone https://github.com/arshadlab/multi_robot_arm.git @@ -29,30 +71,54 @@ rosdep install --from-paths src --ignore-src -r -y colcon build --symlink-install ``` -## Launch -### Console A (Launch Gazebo) +### For ROS2 Jazzy +```bash +source /opt/ros/jazzy/setup.bash +mkdir -p robot_ws/src +cd robot_ws/src +git clone https://github.com/arshadlab/multi_robot_arm.git +cd .. +rosdep install --from-paths src --ignore-src -r -y +sudo apt install ros-jazzy-pymoveit2 +colcon build --symlink-install ``` + +**Note:** For ROS2 Jazzy, pymoveit2 is available as an apt package, so cloning from git is not required. + +## Launch + +### Option 1: ROS2 Humble with Gazebo 11 +#### Console A (Launch Gazebo) +```bash source ./install/setup.bash ros2 launch multi_robot_arm gazebo_arm.launch.py ``` -### Console B (Trigger ARM movement) - -ARM1 movement to position [0.5, 0.4,0.2] using kinematic path planner +### Option 2: ROS2 Jazzy with Gazebo Harmonic (Recommended) +#### Console A (Launch Gazebo Harmonic) +```bash +source ./install/setup.bash +ros2 launch multi_robot_arm gazebo_harmonic_arm.launch.py ``` + +### Triggering ARM Movement +#### Console B (Trigger ARM movement) + +ARM1 movement to position [0.5, 0.4, 0.2] using kinematic path planner: +```bash source ./install/setup.bash cd ./src/pymoveit2/examples -python ex_pose_goal.py --ros-args -r __ns:=/arm1 -p position:=[0.5, 0.4,0.2] +python ex_pose_goal.py --ros-args -r __ns:=/arm1 -p position:=[0.5,0.4,0.2] ``` -ARM4 movement to position [0.5, 0.4,0.2] using cartesian path planner -``` +ARM4 movement to position [0.5, 0.4, 0.2] using cartesian path planner: +```bash source ./install/setup.bash cd ./src/pymoveit2/examples -python ex_pose_goal.py --ros-args -r __ns:=/arm4 -p position:=[0.5, 0.4,0.2] -p cartesian:=True +python ex_pose_goal.py --ros-args -r __ns:=/arm4 -p position:=[0.5,0.4,0.2] -p cartesian:=True ``` -__ns:=/namespace parameter is to direct commands to a specific instance of robot. Position coordinates are given relative to arm position. +**Note:** The `__ns:=/namespace` parameter directs commands to a specific instance of robot. Position coordinates are given relative to arm position. ## Acknowledgement -pymoveit2 -> https://github.com/AndrejOrsula/pymoveit2 +pymoveit2 → https://github.com/AndrejOrsula/pymoveit2 diff --git a/config/ur/ur5/ros_controllers_robot.yaml b/config/ur/ur5/ros_controllers_robot.yaml index 7927877..086745b 100644 --- a/config/ur/ur5/ros_controllers_robot.yaml +++ b/config/ur/ur5/ros_controllers_robot.yaml @@ -44,7 +44,7 @@ arm_controller: wrist_1_joint: { trajectory: 0.25, goal: 0.1 } wrist_2_joint: { trajectory: 0.25, goal: 0.1 } wrist_3_joint: { trajectory: 0.25, goal: 0.1 } - use_sim_time: false + use_sim_time: true forward_velocity_controller: ros__parameters: @@ -58,7 +58,7 @@ forward_velocity_controller: interface_name: - velocity - use_sim_time: false + use_sim_time: true forward_position_controller: ros__parameters: joints: @@ -68,19 +68,4 @@ forward_position_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint - use_sim_time: false -# Gripper controller -rgripper_controller1: - ros__parameters: - joint: robotiq_85_left_knuckle_joint - action_monitor_rate: 20.0 - goal_tolerance: 0.002 - max_effort: 100.0 - stall_velocity_threshold: 0.001 - stall_timeout: 1.0 - command_interfaces: - - position - state_interfaces: - - position - - velocity - use_sim_time: false \ No newline at end of file + use_sim_time: true \ No newline at end of file diff --git a/launch/gazebo_arm.launch.py b/launch/gazebo_arm.launch.py index af03d99..0181a2e 100644 --- a/launch/gazebo_arm.launch.py +++ b/launch/gazebo_arm.launch.py @@ -165,8 +165,8 @@ def spawn_robot( robot_description = {"robot_description": robot_urdf} - kinematics_yaml = load_yaml( - package_path, "config/ur/" + robot_type + "/kinematics.yaml" + kinematics_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/kinematics.yaml") ) robot_description_semantic_config = load_file( @@ -185,22 +185,21 @@ def spawn_robot( }, } - ompl_planning_yaml = load_yaml( - package_path, "config/ur/" + robot_type + "/ompl_planning.yaml" + ompl_planning_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/ompl_planning.yaml") ) ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) - joint_limits_yaml = load_yaml( - package_path, "config/ur/" + robot_type + "/joint_limits_planning.yaml" + joint_limits_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/joint_limits_planning.yaml") ) joint_limits = {"robot_description_planning": joint_limits_yaml} # Trajectory Execution Functionality - moveit_simple_controllers_yaml = load_yaml( - package_path, - "config/ur/" + robot_type + "/moveit_controller_manager.yaml" + moveit_simple_controllers_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/moveit_controller_manager.yaml") ) moveit_controllers = { @@ -402,12 +401,3 @@ def load_file(package_path, file_path): return file.read() except EnvironmentError: return None - -def load_yaml(package_path, file_path): - - 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: - return None diff --git a/launch/gazebo_harmonic_arm.launch.py b/launch/gazebo_harmonic_arm.launch.py new file mode 100644 index 0000000..a635172 --- /dev/null +++ b/launch/gazebo_harmonic_arm.launch.py @@ -0,0 +1,419 @@ +# Copyright (c) 2026 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "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. + +''' +ROS2 Jazzy launch file to spawn multiple UR5 robot arms in Gazebo Harmonic +''' + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription +from launch.event_handlers import OnProcessExit +from launch.launch_context import LaunchContext +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from nav2_common.launch import RewrittenYaml +import yaml, xacro + + + +def generate_launch_description(): + + + package_path = get_package_share_directory("multi_robot_arm") + + declare_use_sim_time = DeclareLaunchArgument( + name="use_sim_time", default_value='true', description="Use simulator time" + ) + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + + + robot_type = "ur5" # ROBOT_MODEL #LaunchConfiguration("robot") + + world = LaunchConfiguration("world") + declare_world_path = DeclareLaunchArgument( + "world", + default_value=os.path.join( + package_path, + "worlds", + "empty.sdf" + ), + description="Full path to world model file to load", + ) + + + declare_robot_type = DeclareLaunchArgument( + name="robot_type", default_value=robot_type, description="Robot type" + ) + + + # Gazebo Harmonic launch + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + get_package_share_directory('ros_gz_sim'), + '/launch/gz_sim.launch.py' + ]), + launch_arguments={ + 'gz_args': [world, ' -v 4'], + 'on_exit_shutdown': 'true' + }.items() + ) + + + ld = LaunchDescription() + ld.add_action(declare_world_path) + ld.add_action(declare_robot_type) + ld.add_action(declare_use_sim_time) + ld.add_action(gz_sim) + + + robots = [ + {'name': 'arm1', 'x_pose': '-1.5', 'y_pose': '-1.50', 'Y':'0.0'}, + {'name': 'arm2', 'x_pose': '-1.5', 'y_pose': '1.5', 'Y':'0.0'}, + {'name': 'arm3', 'x_pose': '1.5', 'y_pose': '-1.5', 'Y':'-3.14'}, + {'name': 'arm4', 'x_pose': '1.5', 'y_pose': '1.5', 'Y':'-3.14'}, + # … + # … + ] + + + # Multiple ARMs in gazebo must be spawned in a serial fashion due to + # a global namespace dependency introduced by ros2_control. + # robot_final_action is the last action from previous robot and + # new robot will only be spawned once previous action is completed + robot_final_action = None + for robot in robots: + robot_final_action = spawn_robot( + ld, + "ur5", + robot["name"] , + use_sim_time, + robot["x_pose"], + robot["y_pose"], + robot["Y"], + robot_final_action, + ) + + + return ld + + + +def spawn_robot( + ld, robot_type, robot_name, use_sim_time, x, y, Y, + previous_final_action=None): + + + package_path = get_package_share_directory("multi_robot_arm") + namespace = "/" + robot_name + + + param_substitutions = {"use_sim_time": use_sim_time} + configured_params = RewrittenYaml( + source_file=package_path + + "/config/ur/" + robot_type + "/ros_controllers_robot.yaml", + root_key=robot_name, + param_rewrites=param_substitutions, + convert_types=True, + ) + + + context = LaunchContext() + controller_paramfile = configured_params.perform(context) + xacro_path = os.path.join(package_path, "urdf", "ur", "ur5", "ur_urdf.xacro") + + + robot_doc = xacro.process_file( + xacro_path, + mappings={ + "name": robot_name, + "namespace": namespace, + "sim_gazebo": "1", + "simulation_controllers": controller_paramfile, + "safety_limits": "true", + "prefix": "", + "pedestal_height": "0.1", + }, + ) + + + robot_urdf = robot_doc.toprettyxml(indent=" ") + + + + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + + robot_params = {"robot_description": robot_urdf, + "use_sim_time": use_sim_time} + robot_state_publisher = Node( + package="robot_state_publisher", + namespace=namespace, + executable="robot_state_publisher", + output="screen", + remappings=remappings, + parameters=[robot_params], + ) + + + robot_description = {"robot_description": robot_urdf} + + + kinematics_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/kinematics.yaml") + ) + + + robot_description_semantic_config = load_file( + package_path, "config/ur/" + robot_type + "/robot.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + + # Planning Functionality + ompl_planning_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/ompl_planning.yaml") + ) + + ompl_planning_pipeline_config = { + "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, + } + } + + ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + + + joint_limits_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/joint_limits_planning.yaml") + ) + + + joint_limits = {"robot_description_planning": joint_limits_yaml} + + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = xacro.load_yaml( + os.path.join(package_path, "config/ur/" + robot_type + "/moveit_controller_manager.yaml") + ) + + + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_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, + "trajectory_execution.execution_duration_monitoring": True, + "trajectory_execution.controller_connection_timeout": 30.0, + } + + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + "use_sim_time": use_sim_time, + } + + + planning_pipelines = { + "planning_pipelines": ["ompl"], + "default_planning_pipeline": "ompl", + } + + + # Start the actual move_group node/action server + robot_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + namespace=namespace, + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + joint_limits, + planning_pipelines, + {"use_sim_time": use_sim_time}, + ], + remappings=remappings, + arguments=["--ros-args", "--log-level", "info"], + ) + + + controller_run_state = 'active' + + + # Use ros_gz_sim spawn_entity for Gazebo Harmonic + robot_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-string", robot_urdf, + "-name", robot_name, + "-x", x, + "-y", y, + "-z", "0.0", + "-Y", Y, + ], + output="screen", + ) + + + load_joint_state_controller = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + controller_run_state, + "joint_state_broadcaster", + "-c", + namespace + "/controller_manager", + ], + output="screen", + ) + + + load_arm_trajectory_controller = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + controller_run_state, + "arm_controller", + "-c", + namespace + "/controller_manager", + ], + output="screen", + ) + message = """ { + 'header': { + 'stamp': { + 'sec': 0, + 'nanosec': 0 + }, + 'frame_id': '' + }, + 'joint_names': [ + 'shoulder_pan_joint', + 'shoulder_lift_joint', + 'elbow_joint', + 'wrist_1_joint', + 'wrist_2_joint', + 'wrist_3_joint' + ], + 'points': [ + { + 'positions': [0.0, -0.97, 2.0, -2.56, -1.55, 0.0], + 'velocities': [], + 'accelerations': [], + 'effort': [], + 'time_from_start': { + 'sec': 1, + 'nanosec': 0 + } + } + ] + }""" + + + # Set initial joint position for robot. This step is not needed for Humble + # In Humble, initial positions are taken from initial_positions.yaml and set by ros2 control plugin + set_initial_pose = ExecuteProcess( + cmd=[ + "ros2", + "topic", + "pub", + "--once", + "/" + robot_name + "/arm_controller/joint_trajectory", + "trajectory_msgs/msg/JointTrajectory", + message, + ], + output="screen", + ) + + if previous_final_action is not None: + spawn_entity = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=previous_final_action, + on_exit=[robot_spawn_entity], + ) + ) + else: + spawn_entity = robot_spawn_entity + + + state_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_spawn_entity, + on_exit=[load_joint_state_controller], + ) + ) + arm_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_arm_trajectory_controller], + ) + ) + + + set_initial_pose_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_arm_trajectory_controller, + on_exit=[set_initial_pose], + ) + ) + + + ld.add_action(robot_state_publisher) + ld.add_action(robot_move_group_node) + ld.add_action(spawn_entity) + ld.add_action(state_controller_event) + ld.add_action(arm_controller_event) + ld.add_action(set_initial_pose_event) + + + return load_arm_trajectory_controller + + + +def load_file(package_path, file_path): + + + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: + return None diff --git a/package.xml b/package.xml index a4a5b97..050cb6c 100644 --- a/package.xml +++ b/package.xml @@ -2,9 +2,10 @@ multi_robot_arm - 1.0.0 - This package demonstrate spawning multiple Robotic arms in Gazebo simulation environment separated by their own namespace + 2.0.0 + This package demonstrate spawning multiple Robotic arms in Gazebo Harmonic simulation environment separated by their own namespace Arshad Mehmood + Jayabalaji Sathiyamoorthi Apache 2.0 xacro @@ -16,7 +17,9 @@ geometry_msgs moveit_msgs std_srvs - gazebo_ros2_control + ros_gz_sim + ros_gz_sim_demos + gz_ros2_control trajectory_msgs controller_interface controller_manager @@ -26,6 +29,7 @@ robot_state_publisher joint_trajectory_controller ros2controlcli + pymoveit2 ament_lint_auto ament_lint_common diff --git a/run.mp4 b/run.mp4 new file mode 100644 index 0000000..8083d79 Binary files /dev/null and b/run.mp4 differ diff --git a/scripts/activate_controllers.py b/scripts/activate_controllers.py new file mode 100644 index 0000000..7fcb905 --- /dev/null +++ b/scripts/activate_controllers.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +""" +Utility script to check and activate controllers for all arms. +Usage: python3 activate_controllers.py +""" + +import subprocess +import sys +from typing import Dict + + +def _run(cmd, timeout=10): + """Run subprocess and return result.""" + return subprocess.run(cmd, check=False, capture_output=True, text=True, timeout=timeout) + + +def _controllers_state(arm_name: str) -> Dict[str, str]: + """Return controller -> state for an arm.""" + res = _run([ + "ros2", "control", "list_controllers", + "-c", f"/{arm_name}/controller_manager", + ]) + state = {} + for line in res.stdout.splitlines(): + parts = line.split() + if len(parts) >= 2: + ctrl, status = parts[0], parts[1] + state[ctrl] = status + return state + + +def check_all_controllers(): + """Check and report all arm controllers.""" + print("\n=== Checking controller states ===") + arms = ["arm1", "arm2", "arm3", "arm4"] + inactive_controllers = {} + + for arm in arms: + print(f"\n{arm}:") + state = _controllers_state(arm) + needed = ["joint_state_broadcaster", "arm_controller"] + for ctrl in needed: + st = state.get(ctrl, "missing") + status = "✓" if st == "active" else "✗" + print(f" {status} {ctrl}: {st}") + if st != "active": + if arm not in inactive_controllers: + inactive_controllers[arm] = [] + inactive_controllers[arm].append(ctrl) + + return inactive_controllers + + +def activate_inactive_controllers(inactive_controllers: Dict[str, list]): + """Activate all inactive controllers.""" + if not inactive_controllers: + print("\n✓ All controllers already active!") + return + + print("\n=== Activating inactive controllers ===") + for arm, ctrls in inactive_controllers.items(): + print(f"\nActivating {arm}: {', '.join(ctrls)}") + + # Try to load missing ones + for ctrl in ctrls: + res = _run([ + "ros2", "control", "load_controller", "--set-state", "active", + ctrl, "-c", f"/{arm}/controller_manager", + ]) + if res.returncode != 0: + print(f" ⚠ Could not load {ctrl}: {res.stderr[:100]}") + + # Attempt to activate both + res = _run([ + "ros2", "control", "switch_controllers", + "--activate", "arm_controller", "joint_state_broadcaster", + "-c", f"/{arm}/controller_manager", + ]) + if res.returncode == 0: + print(f" ✓ Activated controllers for {arm}") + else: + print(f" ✗ Failed to activate: {res.stderr[:100]}") + + # Final check + print("\n=== Final controller states ===") + check_all_controllers() + + +def main(): + print("Checking and activating controllers for all arms...") + inactive = check_all_controllers() + if inactive: + activate_inactive_controllers(inactive) + print("\n✓ Controller activation complete!") + + +if __name__ == "__main__": + main() diff --git a/scripts/dance_arms.py b/scripts/dance_arms.py new file mode 100755 index 0000000..311408b --- /dev/null +++ b/scripts/dance_arms.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +""" +Continuous dancing motion for all arms - cycles through poses A->B->C->D until Ctrl+C +Author: Jb +""" + +import rclpy +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration +import time +import signal +import sys + + +class WavingArmsNode(Node): + def __init__(self): + super().__init__('waving_arms') + + # Create publishers for all 4 arms + self.arm_publishers = {} + for i in range(1, 5): + topic = f'/arm{i}/arm_controller/joint_trajectory' + self.arm_publishers[f'arm{i}'] = self.create_publisher(JointTrajectory, topic, 10) + + # Joint names that work with the controllers + self.joint_names = [ + 'shoulder_pan_joint', + 'shoulder_lift_joint', + 'elbow_joint', + 'wrist_1_joint', + 'wrist_2_joint', + 'wrist_3_joint' + ] + + # Define poses - cycling through A -> B -> C -> D + # [shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3] + self.pose_A = [0.8, -0.8, 1.0, -1.5, -1.5, 0.0] # Base right, arm reaching + self.pose_B = [-0.8, -1.3, 1.8, -0.8, 1.5, 0.0] # Base left, arm high + self.pose_C = [0.0, -0.5, 0.3, -2.0, 0.0, 1.0] # Center, arm forward with wrist twist + self.pose_D = [0.5, -1.8, 2.2, -0.5, -0.5, -1.0] # Base right, dramatic pose with wrist + + self.poses = [self.pose_A, self.pose_B, self.pose_C, self.pose_D] + self.pose_names = ['A', 'B', 'C', 'D'] + self.current_pose_index = 0 + self.running = True + self.get_logger().info('Dance motion ready. Press Ctrl+C to stop.') + + def send_pose_to_all_arms(self, positions): + # Send to all arms simultaneously + for arm_name, publisher in self.arm_publishers.items(): + msg = JointTrajectory() + msg.joint_names = self.joint_names + + point = JointTrajectoryPoint() + point.positions = positions + point.velocities = [0.0] * len(positions) + point.time_from_start = Duration(sec=3) + msg.points = [point] + + publisher.publish(msg) + + def wave_continuously(self): + """Main dancing loop with continuous motion through all poses""" + self.get_logger().info('Starting dance sequence...') + + # Initialize all arms to pose A first + self.send_pose_to_all_arms(self.pose_A) + time.sleep(6) + + while self.running: + try: + # Move to next pose in sequence + self.current_pose_index = (self.current_pose_index + 1) % 4 + next_positions = self.poses[self.current_pose_index] + + # Send to all arms + for publisher in self.arm_publishers.values(): + if not self.running: + break + + msg = JointTrajectory() + msg.joint_names = self.joint_names + + point = JointTrajectoryPoint() + point.positions = next_positions + point.velocities = [0.0] * len(next_positions) + point.time_from_start = Duration(sec=2) + msg.points = [point] + + publisher.publish(msg) + time.sleep(0.1) + + time.sleep(1.0) + + except KeyboardInterrupt: + break + + self.get_logger().info('Returning to home position...') + home_position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + for publisher in self.arm_publishers.values(): + msg = JointTrajectory() + msg.joint_names = self.joint_names + + point = JointTrajectoryPoint() + point.positions = home_position + point.velocities = [0.0] * len(home_position) + point.time_from_start = Duration(sec=3) + msg.points = [point] + + publisher.publish(msg) + time.sleep(4) + + def shutdown(self): + """Clean shutdown""" + self.running = False + + +def signal_handler(sig, frame): + """Handle Ctrl+C gracefully""" + print('\nStopping...') + global node + node.shutdown() + + +def main(): + global node + # Setup signal handler for Ctrl+C + signal.signal(signal.SIGINT, signal_handler) + rclpy.init() + node = WavingArmsNode() + + # Wait for publishers to be ready + time.sleep(1) + try: + # Start the dance motion + node.wave_continuously() + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/urdf/ur/inc/ur_common.xacro b/urdf/ur/inc/ur_common.xacro index 264fd24..a763e78 100644 --- a/urdf/ur/inc/ur_common.xacro +++ b/urdf/ur/inc/ur_common.xacro @@ -45,10 +45,10 @@ - - - - + + + + diff --git a/urdf/ur/ur5/ur_ros2_control.xacro b/urdf/ur/ur5/ur_ros2_control.xacro index d8e8bd1..e38cf8d 100644 --- a/urdf/ur/ur5/ur_ros2_control.xacro +++ b/urdf/ur/ur5/ur_ros2_control.xacro @@ -21,10 +21,10 @@ - gazebo_ros2_control/GazeboSystem + gz_ros2_control/GazeboSimSystem - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem fake_components/GenericSystem @@ -153,17 +153,6 @@ - - - - 0 - - - - - - - diff --git a/urdf/ur/ur5/ur_urdf.xacro b/urdf/ur/ur5/ur_urdf.xacro index af960ed..ffeb215 100644 --- a/urdf/ur/ur5/ur_urdf.xacro +++ b/urdf/ur/ur5/ur_urdf.xacro @@ -74,7 +74,7 @@ sim_gazebo="$(arg sim_gazebo)" sim_ignition="$(arg sim_ignition)" headless_mode="$(arg headless_mode)" - initial_positions="${load_yaml(initial_positions_file)}" + initial_positions="${xacro.load_yaml(initial_positions_file)}" use_tool_communication="$(arg use_tool_communication)" tool_voltage="$(arg tool_voltage)" tool_parity="$(arg tool_parity)" @@ -136,7 +136,7 @@ - + $(arg namespace) @@ -152,7 +152,7 @@ - + $(arg simulation_controllers) $(arg prefix)controller_manager diff --git a/worlds/empty.sdf b/worlds/empty.sdf new file mode 100644 index 0000000..a0e663b --- /dev/null +++ b/worlds/empty.sdf @@ -0,0 +1,69 @@ + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + true + 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 + + + + + 0.001 + 1.0 + + + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + + + + + + 5.46 -4.34 2.87 0 0.275643 2.35619 + orbit + perspective + + + + \ No newline at end of file diff --git a/worlds/empty.world b/worlds/empty.world index 178d85a..2c96764 100644 --- a/worlds/empty.world +++ b/worlds/empty.world @@ -37,8 +37,20 @@ - - model://sun - + + + + true + 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 +