├── env-hooks └── multi_robot_arm.dsv.in ├── setup.cfg ├── urdf └── ur │ ├── meshes │ └── ur5 │ │ └── collision │ │ ├── base.stl │ │ ├── forearm.stl │ │ ├── shoulder.stl │ │ ├── upperarm.stl │ │ ├── wrist1.stl │ │ ├── wrist2.stl │ │ └── wrist3.stl │ ├── inc │ ├── ur_transmissions.xacro │ └── ur_common.xacro │ └── ur5 │ ├── ur_urdf.xacro │ ├── ur_ros2_control.xacro │ └── ur_macro.xacro ├── config └── ur │ └── ur5 │ ├── initial_positions.yaml │ ├── kinematics.yaml │ ├── default_kinematics.yaml │ ├── chomp_planning.yaml │ ├── moveit_controller_manager.yaml │ ├── joint_limits_planning.yaml │ ├── physical_parameters.yaml │ ├── ros_controllers_robot.yaml │ ├── visual_parameters.yaml │ ├── joint_limits.yaml │ ├── robot.srdf │ └── ompl_planning.yaml ├── CMakeLists.txt ├── setup.py ├── worlds └── empty.world ├── package.xml ├── README.md └── launch └── gazebo_arm.launch.py /env-hooks/multi_robot_arm.dsv.in: -------------------------------------------------------------------------------- 1 | prepend-non-duplicate;GAZEBO_MODEL_PATH;share/multi_robot_arm/urdf/ur/meshes 2 | 3 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/multi_robot_arm 3 | [install] 4 | install_scripts=$base/lib/multi_robot_arm 5 | -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/base.stl -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/forearm.stl -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/shoulder.stl -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/upperarm.stl -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/wrist1.stl -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/wrist2.stl -------------------------------------------------------------------------------- /urdf/ur/meshes/ur5/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arshadlab/multi_robot_arm/HEAD/urdf/ur/meshes/ur5/collision/wrist3.stl -------------------------------------------------------------------------------- /config/ur/ur5/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -0.97 3 | elbow_joint: 2.0 4 | wrist_1_joint: -2.56 5 | wrist_2_joint: -1.55 6 | wrist_3_joint: 0.0 7 | -------------------------------------------------------------------------------- /config/ur/ur5/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | # kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin 4 | # kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin 5 | kinematics_solver_search_resolution: 0.0025 6 | kinematics_solver_timeout: 0.05 7 | kinematics_solver_attempts: 5 8 | # max_cache_size: 100000 9 | 10 | 11 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(multi_robot_arm) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(gazebo_ros REQUIRED) 8 | 9 | include_directories(include) 10 | link_directories( 11 | ${gazebo_dev_LIBRARY_DIRS} 12 | ) 13 | 14 | set(THIS_PACKAGE_INCLUDE_DEPENDS 15 | rclcpp 16 | std_msgs 17 | ) 18 | 19 | install(DIRECTORY launch config urdf worlds DESTINATION share/${PROJECT_NAME}) 20 | ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/multi_robot_arm.dsv.in") 21 | 22 | ament_package() 23 | -------------------------------------------------------------------------------- /config/ur/ur5/default_kinematics.yaml: -------------------------------------------------------------------------------- 1 | kinematics: 2 | shoulder: 3 | x: 0 4 | y: 0 5 | z: 0.089159 6 | roll: 0 7 | pitch: 0 8 | yaw: 0 9 | upper_arm: 10 | x: 0 11 | y: 0 12 | z: 0 13 | roll: 1.570796327 14 | pitch: 0 15 | yaw: 0 16 | forearm: 17 | x: -0.425 18 | y: 0 19 | z: 0 20 | roll: 0 21 | pitch: 0 22 | yaw: 0 23 | wrist_1: 24 | x: -0.39225 25 | y: 0 26 | z: 0.10915 27 | roll: 0 28 | pitch: 0 29 | yaw: 0 30 | wrist_2: 31 | x: 0 32 | y: -0.09465 33 | z: -1.941303950897609e-11 34 | roll: 1.570796327 35 | pitch: 0 36 | yaw: 0 37 | wrist_3: 38 | x: 0 39 | y: 0.0823 40 | z: -1.688001216681175e-11 41 | roll: 1.570796326589793 42 | pitch: 3.141592653589793 43 | yaw: 3.141592653589793 44 | hash: calib_209549117540498681 45 | -------------------------------------------------------------------------------- /config/ur/ur5/chomp_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_time_limit: 10.0 2 | max_iterations: 200 3 | max_iterations_after_collision_free: 5 4 | smoothness_cost_weight: 0.1 5 | obstacle_cost_weight: 1.0 6 | learning_rate: 0.01 7 | animate_path: true 8 | add_randomness: false 9 | smoothness_cost_velocity: 0.0 10 | smoothness_cost_acceleration: 1.0 11 | smoothness_cost_jerk: 0.0 12 | hmc_discretization: 0.01 13 | hmc_stochasticity: 0.01 14 | hmc_annealing_factor: 0.99 15 | use_hamiltonian_monte_carlo: false 16 | ridge_factor: 0.0 17 | use_pseudo_inverse: false 18 | pseudo_inverse_ridge_factor: 1e-4 19 | animate_endeffector: false 20 | animate_endeffector_segment: "panda_rightfinger" 21 | joint_update_limit: 0.1 22 | collision_clearence: 0.2 23 | collision_threshold: 0.07 24 | random_jump_amount: 1.0 25 | use_stochastic_descent: true 26 | enable_failure_recovery: false 27 | max_recovery_attempts: 5 28 | trajectory_initialization_method: "quintic-spline" 29 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | package_name = 'robot_config' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | py_modules=[], 10 | package_dir={'': 'scripts'} 11 | install_requires=['setuptools'], 12 | zip_safe=True, 13 | author='Arshad Mehmood', 14 | author_email='arshad.mehmood@intel.com', 15 | maintainer='Arshad Mehmood', 16 | maintainer_email='arshad.mehmood@intel.com', 17 | keywords=['ROS2'], 18 | classifiers=[ 19 | 'Intended Audience :: Developers', 20 | 'License :: OSI Approved :: Apache Software License', 21 | 'Programming Language :: Python', 22 | 'Topic :: Software Development', 23 | ], 24 | description='Examples of minimal subscribers using rclpy.', 25 | license='Apache License, Version 2.0', 26 | entry_points={ 27 | 'console_scripts': [ 28 | 29 | ], 30 | }, 31 | ) 32 | -------------------------------------------------------------------------------- /config/ur/ur5/moveit_controller_manager.yaml: -------------------------------------------------------------------------------- 1 | controller_names: 2 | - arm_controller 3 | 4 | # - gripper_action_controller 5 | arm_controller: 6 | action_ns: follow_joint_trajectory 7 | type: FollowJointTrajectory 8 | default: true 9 | joints: 10 | - shoulder_pan_joint 11 | - shoulder_lift_joint 12 | - elbow_joint 13 | - wrist_1_joint 14 | - wrist_2_joint 15 | - wrist_3_joint 16 | 17 | gripper_controller1: 18 | action_ns: follow_joint_trajectory 19 | type: GripperActionController 20 | default: true 21 | joint: robotiq_85_left_knuckle_joint 22 | 23 | 24 | #gripper_controller: 25 | # joint: panda_finger_joint1 26 | 27 | # ----- Gripper action controller (not yet functional) 28 | # gripper_action_controller: 29 | # action_ns: gripper_cmd 30 | # type: GripperCommand 31 | # default: true 32 | # joints: 33 | # - panda_finger_joint1 34 | # 35 | # Note: Once implemented, the following config needs to be within controller parameters 36 | # controller_manager: 37 | # ros__parameters: 38 | # gripper_action_controller: 39 | # type: position_controllers/GripperActionController 40 | # gripper_action_controller: 41 | # ros__parameters: 42 | # joint: panda_finger_joint1 43 | # 44 | # Node: Remember to enable mimicking of finger joints before attempting to use gripper action 45 | -------------------------------------------------------------------------------- /worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 -9.8 7 | 8 | 0.01 9 | 1 10 | 100 11 | 12 | 13 | 14 | true 15 | 16 | 17 | 18 | 19 | 0 0 1 20 | 100 100 21 | 22 | 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 36 | 37 | 38 | 39 | 40 | 41 | model://sun 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /config/ur/ur5/joint_limits_planning.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | elbow_joint: 6 | has_velocity_limits: true 7 | max_velocity: 1.15 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | robotiq_85_left_knuckle_joint: 11 | has_velocity_limits: true 12 | max_velocity: 0.5 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | shoulder_lift_joint: 16 | has_velocity_limits: true 17 | max_velocity: 1.15 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | shoulder_pan_joint: 21 | has_velocity_limits: true 22 | max_velocity: 1.15 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | wrist_1_joint: 26 | has_velocity_limits: true 27 | max_velocity: 1.2 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | wrist_2_joint: 31 | has_velocity_limits: true 32 | max_velocity: 1.2 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | wrist_3_joint: 36 | has_velocity_limits: true 37 | max_velocity: 1.2 38 | has_acceleration_limits: false 39 | max_acceleration: 0 -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | multi_robot_arm 5 | 1.0.0 6 | This package demonstrate spawning multiple Robotic arms in Gazebo simulation environment separated by their own namespace 7 | Arshad Mehmood 8 | Apache 2.0 9 | 10 | xacro 11 | moveit_core 12 | rclpy 13 | nav2_common 14 | action_msgs 15 | control_msgs 16 | geometry_msgs 17 | moveit_msgs 18 | std_srvs 19 | gazebo_ros2_control 20 | trajectory_msgs 21 | controller_interface 22 | controller_manager 23 | controller_manager_msgs 24 | hardware_interface 25 | joint_state_broadcaster 26 | robot_state_publisher 27 | joint_trajectory_controller 28 | ros2controlcli 29 | 30 | ament_lint_auto 31 | ament_lint_common 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /config/ur/ur5/physical_parameters.yaml: -------------------------------------------------------------------------------- 1 | # Physical parameters 2 | 3 | dh_parameters: 4 | d1: 0.089159 5 | a2: -0.42500 6 | a3: -0.39225 7 | d4: 0.10915 8 | d5: 0.09465 9 | d6: 0.0823 10 | 11 | offsets: 12 | shoulder_offset: 0.13585 # measured from model 13 | elbow_offset: 0.0165 # measured from model 14 | 15 | inertia_parameters: 16 | base_mass: 4.0 # This mass might be incorrect 17 | shoulder_mass: 3.7000 18 | upper_arm_mass: 8.3930 19 | upper_arm_inertia_offset: 0.136 20 | forearm_mass: 2.2750 21 | wrist_1_mass: 1.2190 22 | wrist_2_mass: 1.2190 23 | wrist_3_mass: 0.1879 24 | 25 | shoulder_radius: 0.060 # manually measured 26 | upper_arm_radius: 0.054 # manually measured 27 | elbow_radius: 0.060 # manually measured 28 | forearm_radius: 0.040 # manually measured 29 | wrist_radius: 0.045 # manually measured 30 | 31 | links: 32 | base: 33 | radius: 0.06 34 | length: 0.05 35 | shoulder: 36 | radius: 0.075 37 | length: 0.178 38 | upperarm: 39 | radius: 0.06 40 | length: 0.425 41 | forearm: 42 | radius: 0.06 43 | length: 0.39225 44 | wrist_1: 45 | radius: 0.06 46 | length: 0.12 47 | wrist_2: 48 | radius: 0.06 49 | length: 0.12 50 | wrist_3: 51 | radius: 0.0375 52 | length: 0.0345 53 | wrist_1: 54 | radius: 0.06 55 | length: 0.095 56 | wrist_2: 57 | radius: 0.06 58 | length: 0.085 59 | wrist_3: 60 | radius: 0.0375 61 | length: 0.0305 62 | 63 | center_of_mass: 64 | shoulder_cog: 65 | x: 0.0 66 | y: 0.00193 67 | z: -0.02561 68 | upper_arm_cog: 69 | x: 0.0 70 | y: -0.024201 71 | z: 0.2125 72 | forearm_cog: 73 | x: 0.0 74 | y: 0.0265 75 | z: 0.11993 76 | wrist_1_cog: 77 | x: 0.0 78 | y: 0.110949 79 | z: 0.01634 80 | wrist_2_cog: 81 | x: 0.0 82 | y: 0.0018 83 | z: 0.11099 84 | wrist_3_cog: 85 | x: 0.0 86 | y: 0.001159 87 | z: 0.0 88 | -------------------------------------------------------------------------------- /config/ur/ur5/ros_controllers_robot.yaml: -------------------------------------------------------------------------------- 1 | 2 | controller_manager: 3 | ros__parameters: 4 | update_rate: 100 # Hz 5 | use_sim_time: true 6 | 7 | arm_controller: 8 | type: joint_trajectory_controller/JointTrajectoryController 9 | 10 | joint_state_broadcaster: 11 | type: joint_state_broadcaster/JointStateBroadcaster 12 | 13 | forward_velocity_controller: 14 | type: velocity_controllers/JointGroupVelocityController 15 | 16 | forward_position_controller: 17 | type: position_controllers/JointGroupPositionController 18 | 19 | arm_controller: 20 | ros__parameters: 21 | joints: 22 | - shoulder_pan_joint 23 | - shoulder_lift_joint 24 | - elbow_joint 25 | - wrist_1_joint 26 | - wrist_2_joint 27 | - wrist_3_joint 28 | command_interfaces: 29 | - position 30 | - velocity 31 | state_interfaces: 32 | - position 33 | - velocity 34 | type: joint_trajectory_controller/JointTrajectoryController 35 | state_publish_rate: 100.0 36 | action_monitor_rate: 20.0 37 | allow_partial_joints_goal: false 38 | constraints: 39 | stopped_velocity_tolerance: 0.0 40 | goal_time: 0.0 41 | shoulder_pan_joint: { trajectory: 0.25, goal: 0.1 } 42 | shoulder_lift_joint: { trajectory: 0.25, goal: 0.1 } 43 | elbow_joint: { trajectory: 0.25, goal: 0.1 } 44 | wrist_1_joint: { trajectory: 0.25, goal: 0.1 } 45 | wrist_2_joint: { trajectory: 0.25, goal: 0.1 } 46 | wrist_3_joint: { trajectory: 0.25, goal: 0.1 } 47 | use_sim_time: false 48 | 49 | forward_velocity_controller: 50 | ros__parameters: 51 | joints: 52 | - shoulder_pan_joint 53 | - shoulder_lift_joint 54 | - elbow_joint 55 | - wrist_1_joint 56 | - wrist_2_joint 57 | - wrist_3_joint 58 | 59 | interface_name: 60 | - velocity 61 | use_sim_time: false 62 | forward_position_controller: 63 | ros__parameters: 64 | joints: 65 | - shoulder_pan_joint 66 | - shoulder_lift_joint 67 | - elbow_joint 68 | - wrist_1_joint 69 | - wrist_2_joint 70 | - wrist_3_joint 71 | use_sim_time: false 72 | # Gripper controller 73 | rgripper_controller1: 74 | ros__parameters: 75 | joint: robotiq_85_left_knuckle_joint 76 | action_monitor_rate: 20.0 77 | goal_tolerance: 0.002 78 | max_effort: 100.0 79 | stall_velocity_threshold: 0.001 80 | stall_timeout: 1.0 81 | command_interfaces: 82 | - position 83 | state_interfaces: 84 | - position 85 | - velocity 86 | use_sim_time: false -------------------------------------------------------------------------------- /config/ur/ur5/visual_parameters.yaml: -------------------------------------------------------------------------------- 1 | mesh_files: 2 | base: 3 | visual: 4 | mesh: 5 | package: multi_robot_arm 6 | path: urdf/ur/meshes/ur5/visual/base.dae 7 | material: 8 | name: "LightGrey" 9 | color: "0.7 0.7 0.7 1.0" 10 | collision: 11 | mesh: 12 | package: multi_robot_arm 13 | path: urdf/ur/meshes/ur5/collision/base.stl 14 | 15 | shoulder: 16 | visual: 17 | mesh: 18 | package: multi_robot_arm 19 | path: urdf/ur/meshes/ur5/visual/shoulder.dae 20 | material: 21 | name: "LightGrey" 22 | color: "0.7 0.7 0.7 1.0" 23 | collision: 24 | mesh: 25 | package: multi_robot_arm 26 | path: urdf/ur/meshes/ur5/collision/shoulder.stl 27 | 28 | upper_arm: 29 | visual: 30 | mesh: 31 | package: multi_robot_arm 32 | path: urdf/ur/meshes/ur5/visual/upperarm.dae 33 | material: 34 | name: "LightGrey" 35 | color: "0.7 0.7 0.7 1.0" 36 | collision: 37 | mesh: 38 | package: multi_robot_arm 39 | path: urdf/ur/meshes/ur5/collision/upperarm.stl 40 | mesh_files: 41 | 42 | forearm: 43 | visual: 44 | mesh: 45 | package: multi_robot_arm 46 | path: urdf/ur/meshes/ur5/visual/forearm.dae 47 | material: 48 | name: "LightGrey" 49 | color: "0.7 0.7 0.7 1.0" 50 | collision: 51 | mesh: 52 | package: multi_robot_arm 53 | path: urdf/ur/meshes/ur5/collision/forearm.stl 54 | 55 | wrist_1: 56 | visual: 57 | mesh: 58 | package: multi_robot_arm 59 | path: urdf/ur/meshes/ur5/visual/wrist1.dae 60 | material: 61 | name: "LightGrey" 62 | color: "0.7 0.7 0.7 1.0" 63 | collision: 64 | mesh: 65 | package: multi_robot_arm 66 | path: urdf/ur/meshes/ur5/collision/wrist1.stl 67 | visual_offset: -0.093 68 | 69 | wrist_2: 70 | visual: 71 | mesh: 72 | package: multi_robot_arm 73 | path: urdf/ur/meshes/ur5/visual/wrist2.dae 74 | material: 75 | name: "LightGrey" 76 | color: "0.7 0.7 0.7 1.0" 77 | collision: 78 | mesh: 79 | package: multi_robot_arm 80 | path: urdf/ur/meshes/ur5/collision/wrist2.stl 81 | visual_offset: -0.095 82 | 83 | wrist_3: 84 | visual: 85 | mesh: 86 | package: multi_robot_arm 87 | path: urdf/ur/meshes/ur5/visual/wrist3.dae 88 | material: 89 | name: "LightGrey" 90 | color: "0.7 0.7 0.7 1.0" 91 | collision: 92 | mesh: 93 | package: multi_robot_arm 94 | path: urdf/ur/meshes/ur5/collision/wrist3.stl 95 | visual_offset: -0.0818 96 | -------------------------------------------------------------------------------- /urdf/ur/inc/ur_transmissions.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | 11 | 12 | transmission_interface/SimpleTransmission 13 | 14 | ${hw_interface} 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | transmission_interface/SimpleTransmission 23 | 24 | ${hw_interface} 25 | 26 | 27 | 1 28 | 29 | 30 | 31 | 32 | transmission_interface/SimpleTransmission 33 | 34 | ${hw_interface} 35 | 36 | 37 | 1 38 | 39 | 40 | 41 | 42 | transmission_interface/SimpleTransmission 43 | 44 | ${hw_interface} 45 | 46 | 47 | 1 48 | 49 | 50 | 51 | 52 | transmission_interface/SimpleTransmission 53 | 54 | ${hw_interface} 55 | 56 | 57 | 1 58 | 59 | 60 | 61 | 62 | transmission_interface/SimpleTransmission 63 | 64 | ${hw_interface} 65 | 66 | 67 | 1 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /config/ur/ur5/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # Joints limits 2 | # 3 | # Sources: 4 | # 5 | # - UR5 User Manual, Universal Robots, UR5/CB3, Version 3.13 6 | # https://s3-eu-west-1.amazonaws.com/ur-support-site/69371/99202_UR5_User_Manual_en_Global.pdf 7 | # - Support > Articles > UR articles > Max. joint torques 8 | # https://www.universal-robots.com/articles/ur-articles/max-joint-torques 9 | # retrieved: 2020-06-16, last modified: 2020-06-09 10 | joint_limits: 11 | shoulder_pan_joint: 12 | # acceleration limits are not publicly available 13 | has_acceleration_limits: false 14 | has_effort_limits: true 15 | has_position_limits: true 16 | has_velocity_limits: true 17 | max_effort: 150.0 18 | max_position: !degrees 360.0 19 | max_velocity: !degrees 180.0 20 | min_position: !degrees -360.0 21 | shoulder_lift_joint: 22 | # acceleration limits are not publicly available 23 | has_acceleration_limits: false 24 | has_effort_limits: true 25 | has_position_limits: true 26 | has_velocity_limits: true 27 | max_effort: 150.0 28 | max_position: !degrees 360.0 29 | max_velocity: !degrees 180.0 30 | min_position: !degrees -360.0 31 | elbow_joint: 32 | # acceleration limits are not publicly available 33 | has_acceleration_limits: false 34 | has_effort_limits: true 35 | has_position_limits: true 36 | has_velocity_limits: true 37 | max_effort: 150.0 38 | # we artificially limit this joint to half its actual joint position limit 39 | # to avoid (MoveIt/OMPL) planning problems, as due to the physical 40 | # construction of the robot, it's impossible to rotate the 'elbow_joint' 41 | # over more than approx +- 1 pi (the shoulder lift joint gets in the way). 42 | # 43 | # This leads to planning problems as the search space will be divided into 44 | # two sections, with no connections from one to the other. 45 | # 46 | # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for 47 | # more information. 48 | max_position: !degrees 180.0 49 | max_velocity: !degrees 180.0 50 | min_position: !degrees -180.0 51 | wrist_1_joint: 52 | # acceleration limits are not publicly available 53 | has_acceleration_limits: false 54 | has_effort_limits: true 55 | has_position_limits: true 56 | has_velocity_limits: true 57 | max_effort: 28.0 58 | max_position: !degrees 360.0 59 | max_velocity: !degrees 180.0 60 | min_position: !degrees -360.0 61 | wrist_2_joint: 62 | # acceleration limits are not publicly available 63 | has_acceleration_limits: false 64 | has_effort_limits: true 65 | has_position_limits: true 66 | has_velocity_limits: true 67 | max_effort: 28.0 68 | max_position: !degrees 360.0 69 | max_velocity: !degrees 180.0 70 | min_position: !degrees -360.0 71 | wrist_3_joint: 72 | # acceleration limits are not publicly available 73 | has_acceleration_limits: false 74 | has_effort_limits: true 75 | has_position_limits: true 76 | has_velocity_limits: true 77 | max_effort: 28.0 78 | max_position: !degrees 360.0 79 | max_velocity: !degrees 180.0 80 | min_position: !degrees -360.0 81 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Enabling Multi-Robot ARM in Gazebo for ROS2 2 | This git repository presents an ROS2 package that demonstrates the simultaneous spawning of multiple robotic arms in a Gazebo simulation. Historically, such an application is difficult to come by within the ROS2 community, leaving a knowledge gap for developers looking to implement multi-arm robotic systems. The work outlined here not only fills this void but also offers a practical guide for beginners and interested parties looking to replicate similar multi-robot simulations in Gazebo. 3 | 4 | The UR5 robotic arm, a versatile and widely-used model in robotics, serves as the core of this demonstration. By focusing on this particular model, the tutorial ensures a broad relevance to a multitude of potential robotic applications. 5 | 6 | ![image](https://github.com/arshadlab/multi_robot_arm/assets/85929438/4e052e79-65c7-4fe5-b73a-871b76b9f01e) 7 | ![image](https://github.com/arshadlab/multi_robot_arm/assets/85929438/3189c420-33c1-424c-aaa4-0cc2b8cc868c) 8 | 9 | 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. 10 | 11 | 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. 12 | 13 | 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. 14 | 15 | ## Setup 16 | The package is verified with ROS2 Foxy and Humble. 17 | 18 | Dependencies: ROS2 [Foxy or Humble], moveit2, pymoveit2, Gazebo 11 19 | 20 | ## Clone and Build 21 | ``` 22 | source /opt/ros//setup.bash 23 | mkdir -p robot_ws/src 24 | cd robot_ws/src 25 | git clone https://github.com/arshadlab/multi_robot_arm.git 26 | git clone https://github.com/arshadlab/pymoveit2.git 27 | cd .. 28 | rosdep install --from-paths src --ignore-src -r -y 29 | colcon build --symlink-install 30 | ``` 31 | 32 | ## Launch 33 | ### Console A (Launch Gazebo) 34 | ``` 35 | source ./install/setup.bash 36 | ros2 launch multi_robot_arm gazebo_arm.launch.py 37 | ``` 38 | 39 | ### Console B (Trigger ARM movement) 40 | 41 | ARM1 movement to position [0.5, 0.4,0.2] using kinematic path planner 42 | ``` 43 | source ./install/setup.bash 44 | cd ./src/pymoveit2/examples 45 | python ex_pose_goal.py --ros-args -r __ns:=/arm1 -p position:=[0.5, 0.4,0.2] 46 | ``` 47 | 48 | ARM4 movement to position [0.5, 0.4,0.2] using cartesian path planner 49 | ``` 50 | source ./install/setup.bash 51 | cd ./src/pymoveit2/examples 52 | python ex_pose_goal.py --ros-args -r __ns:=/arm4 -p position:=[0.5, 0.4,0.2] -p cartesian:=True 53 | ``` 54 | 55 | __ns:=/namespace parameter is to direct commands to a specific instance of robot. Position coordinates are given relative to arm position. 56 | 57 | ## Acknowledgement 58 | pymoveit2 -> https://github.com/AndrejOrsula/pymoveit2 59 | -------------------------------------------------------------------------------- /config/ur/ur5/robot.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /urdf/ur/ur5/ur_urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 0.2 128 | 0.2 129 | Gazebo/Orange 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | $(arg namespace) 142 | 143 | $(arg simulation_controllers) 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | $(arg simulation_controllers) 157 | $(arg prefix)controller_manager 158 | 159 | 160 | 161 | 162 | 163 | 164 | -------------------------------------------------------------------------------- /config/ur/ur5/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | FMTkConfigDefault: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMTkConfigDefault: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDSTkConfigDefault: 74 | type: geometric::PDST 75 | STRIDEkConfigDefault: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRTkConfigDefault: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRTkConfigDefault: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiESTkConfigDefault: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjESTkConfigDefault: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRMkConfigDefault: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstarkConfigDefault: 110 | type: geometric::LazyPRMstar 111 | SPARSkConfigDefault: 112 | type: geometric::SPARS 113 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwokConfigDefault: 118 | type: geometric::SPARStwo 119 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | TrajOptDefault: 124 | type: geometric::TrajOpt 125 | projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) 126 | arm: 127 | default_planner_config: RRTConnectkConfigDefault 128 | planner_configs: 129 | - SBLkConfigDefault 130 | - ESTkConfigDefault 131 | - LBKPIECEkConfigDefault 132 | - BKPIECEkConfigDefault 133 | - KPIECEkConfigDefault 134 | - RRTkConfigDefault 135 | - RRTConnectkConfigDefault 136 | - RRTstarkConfigDefault 137 | - TRRTkConfigDefault 138 | - PRMkConfigDefault 139 | - PRMstarkConfigDefault 140 | - FMTkConfigDefault 141 | - BFMTkConfigDefault 142 | - PDSTkConfigDefault 143 | - STRIDEkConfigDefault 144 | - BiTRRTkConfigDefault 145 | - LBTRRTkConfigDefault 146 | - BiESTkConfigDefault 147 | - ProjESTkConfigDefault 148 | - LazyPRMkConfigDefault 149 | - LazyPRMstarkConfigDefault 150 | - SPARSkConfigDefault 151 | - SPARStwokConfigDefault 152 | - TrajOptDefault 153 | longest_valid_segment_fraction: 0.05 154 | 155 | path_tolerance: 0.100 156 | resample_dt: 0.1000 157 | min_angle_change: 0.0010 158 | default_workspace_bounds: 10.00 159 | start_state_max_bounds_error: 0.100 160 | start_state_max_dt: 0.500 161 | jiggle_fraction: 0.020 162 | max_sampling_attempts: 100 163 | -------------------------------------------------------------------------------- /launch/gazebo_arm.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | from ament_index_python.packages import get_package_share_directory 17 | from launch import LaunchDescription 18 | from launch.substitutions import LaunchConfiguration 19 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler 20 | from launch.event_handlers import OnProcessExit 21 | from launch.launch_context import LaunchContext 22 | from launch_ros.actions import Node 23 | from nav2_common.launch import RewrittenYaml 24 | import yaml, xacro 25 | 26 | #https://github.com/bponsler/ros2-support 27 | 28 | def generate_launch_description(): 29 | 30 | package_path = get_package_share_directory("multi_robot_arm") 31 | 32 | declare_use_sim_time = DeclareLaunchArgument( 33 | name="use_sim_time", default_value='true', description="Use simulator time" 34 | ) 35 | use_sim_time = LaunchConfiguration("use_sim_time", default="true") 36 | 37 | robot_type = "ur5" # ROBOT_MODEL #LaunchConfiguration("robot") 38 | 39 | world = LaunchConfiguration("world") 40 | declare_world_path = DeclareLaunchArgument( 41 | "world", 42 | default_value=os.path.join( 43 | package_path, 44 | "worlds", 45 | "empty.world" 46 | ), 47 | description="Full path to world model file to load", 48 | ) 49 | 50 | declare_world_path = DeclareLaunchArgument( 51 | "world", 52 | default_value=os.path.join( 53 | package_path, 54 | "worlds", 55 | "empty.world", 56 | ), 57 | description="Full path to world model file to load", 58 | ) 59 | 60 | declare_robot_type = DeclareLaunchArgument( 61 | name="robot_type", default_value=robot_type, description="Robot type" 62 | ) 63 | 64 | gazebo_server = ExecuteProcess( 65 | cmd=[ 66 | "gzserver", 67 | "--verbose", 68 | "-u", 69 | "-s", "libgazebo_ros_factory.so", 70 | "-s", "libgazebo_ros_init.so", 71 | world, 72 | ], 73 | output="screen", 74 | ) 75 | gazebo_client = ExecuteProcess(cmd=["gzclient"], output="screen") 76 | 77 | 78 | 79 | 80 | ld = LaunchDescription() 81 | ld.add_action(declare_world_path) 82 | ld.add_action(declare_robot_type) 83 | ld.add_action(declare_use_sim_time) 84 | ld.add_action(gazebo_server) 85 | ld.add_action(gazebo_client) 86 | 87 | robots = [ 88 | {'name': 'arm1', 'x_pose': '-1.5', 'y_pose': '-1.50', 'Y':'0.0'}, 89 | {'name': 'arm2', 'x_pose': '-1.5', 'y_pose': '1.5', 'Y':'0.0'}, 90 | {'name': 'arm3', 'x_pose': '1.5', 'y_pose': '-1.5', 'Y':'-3.14'}, 91 | {'name': 'arm4', 'x_pose': '1.5', 'y_pose': '1.5', 'Y':'-3.14'}, 92 | # … 93 | # … 94 | ] 95 | 96 | # Multiple ARMs in gazebo must be spawned in a serial fashion due to 97 | # a global namespace dependency introduced by ros2_control. 98 | # robot_final_action is the last action from previous robot and 99 | # new robot will only be spawned once previous action is completed 100 | robot_final_action = None 101 | for robot in robots: 102 | robot_final_action = spawn_robot( 103 | ld, 104 | "ur5", 105 | robot["name"] , 106 | use_sim_time, 107 | robot["x_pose"], 108 | robot["y_pose"], 109 | robot["Y"], 110 | robot_final_action, 111 | ) 112 | 113 | 114 | return ld 115 | 116 | 117 | def spawn_robot( 118 | ld, robot_type, robot_name, use_sim_time, x, y, Y, 119 | previous_final_action=None): 120 | 121 | package_path = get_package_share_directory("multi_robot_arm") 122 | namespace = "/" + robot_name 123 | 124 | param_substitutions = {"use_sim_time": use_sim_time} 125 | configured_params = RewrittenYaml( 126 | source_file=package_path 127 | + "/config/ur/" + robot_type + "/ros_controllers_robot.yaml", 128 | root_key=robot_name, 129 | param_rewrites=param_substitutions, 130 | convert_types=True, 131 | ) 132 | 133 | context = LaunchContext() 134 | controller_paramfile = configured_params.perform(context) 135 | xacro_path = os.path.join(package_path, "urdf", "ur", "ur5", "ur_urdf.xacro") 136 | 137 | robot_doc = xacro.process_file( 138 | xacro_path, 139 | mappings={ 140 | "name": robot_name, 141 | "namespace": namespace, 142 | "sim_gazebo": "1", 143 | "simulation_controllers": controller_paramfile, 144 | "safety_limits": "true", 145 | "prefix": "", 146 | "pedestal_height": "0.1", 147 | }, 148 | ) 149 | 150 | robot_urdf = robot_doc.toprettyxml(indent=" ") 151 | 152 | 153 | remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] 154 | 155 | robot_params = {"robot_description": robot_urdf, 156 | "use_sim_time": use_sim_time} 157 | robot_state_publisher = Node( 158 | package="robot_state_publisher", 159 | namespace=namespace, 160 | executable="robot_state_publisher", 161 | output="screen", 162 | remappings=remappings, 163 | parameters=[robot_params], 164 | ) 165 | 166 | robot_description = {"robot_description": robot_urdf} 167 | 168 | kinematics_yaml = load_yaml( 169 | package_path, "config/ur/" + robot_type + "/kinematics.yaml" 170 | ) 171 | 172 | robot_description_semantic_config = load_file( 173 | package_path, "config/ur/" + robot_type + "/robot.srdf" 174 | ) 175 | robot_description_semantic = { 176 | "robot_description_semantic": robot_description_semantic_config 177 | } 178 | 179 | # Planning Functionality 180 | ompl_planning_pipeline_config = { 181 | "ompl": { 182 | "planning_plugin": "ompl_interface/OMPLPlanner", 183 | "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", 184 | "start_state_max_bounds_error": 0.1, 185 | }, 186 | } 187 | 188 | ompl_planning_yaml = load_yaml( 189 | package_path, "config/ur/" + robot_type + "/ompl_planning.yaml" 190 | ) 191 | 192 | ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) 193 | 194 | joint_limits_yaml = load_yaml( 195 | package_path, "config/ur/" + robot_type + "/joint_limits_planning.yaml" 196 | ) 197 | 198 | joint_limits = {"robot_description_planning": joint_limits_yaml} 199 | 200 | # Trajectory Execution Functionality 201 | moveit_simple_controllers_yaml = load_yaml( 202 | package_path, 203 | "config/ur/" + robot_type + "/moveit_controller_manager.yaml" 204 | ) 205 | 206 | moveit_controllers = { 207 | "moveit_simple_controller_manager": moveit_simple_controllers_yaml, 208 | "moveit_controller_manager": 209 | "moveit_simple_controller_manager/MoveItSimpleControllerManager", 210 | } 211 | 212 | trajectory_execution = { 213 | "moveit_manage_controllers": True, 214 | "trajectory_execution.allowed_execution_duration_scaling": 1.2, 215 | "trajectory_execution.allowed_goal_duration_margin": 0.5, 216 | "trajectory_execution.allowed_start_tolerance": 0.01, 217 | "trajectory_execution.execution_duration_monitoring": True, 218 | "trajectory_execution.controller_connection_timeout": 30.0, 219 | } 220 | 221 | planning_scene_monitor_parameters = { 222 | "publish_planning_scene": True, 223 | "publish_geometry_updates": True, 224 | "publish_state_updates": True, 225 | "publish_transforms_updates": True, 226 | "default_planning_pipeline": "ESTkConfigDefault", 227 | "use_sim_time": use_sim_time, 228 | } 229 | 230 | pipeline_names = {"pipeline_names": ["ompl"]} 231 | 232 | planning_pipelines = { 233 | "planning_pipelines": pipeline_names, 234 | "default_planning_pipeline": "ompl", 235 | } 236 | 237 | # https://industrial-training-master.readthedocs.io/en/foxy/_source/session3/ros2/3-Build-a-MoveIt-Package.html 238 | # Start the actual move_group node/action server 239 | robot_move_group_node = Node( 240 | package="moveit_ros_move_group", 241 | executable="move_group", 242 | namespace=namespace, 243 | output="screen", 244 | parameters=[ 245 | robot_description, 246 | robot_description_semantic, 247 | kinematics_yaml, 248 | ompl_planning_pipeline_config, 249 | trajectory_execution, 250 | moveit_controllers, 251 | planning_scene_monitor_parameters, 252 | joint_limits, 253 | planning_pipelines, 254 | {"planning_plugin": "ompl", "use_sim_time": use_sim_time}, 255 | ], 256 | remappings=remappings, 257 | arguments=["--ros-args", "--log-level", "info"], 258 | ) 259 | 260 | 261 | ros_distro = os.environ.get('ROS_DISTRO') 262 | 263 | # ROS2 Controller Manager in Foxy uses 'start' while Humble version expects 'active' 264 | 265 | controller_run_state = 'active' 266 | if ros_distro == 'foxy': 267 | controller_run_state = 'start' 268 | 269 | robot_spawn_entity = Node( 270 | package="gazebo_ros", 271 | executable="spawn_entity.py", 272 | arguments=[ 273 | "-topic", namespace + "/robot_description", 274 | "-entity", robot_name, 275 | "-robot_namespace", namespace, 276 | "-x", x, 277 | "-y", y, 278 | "-z", "0.0", 279 | "-Y", Y, 280 | "-unpause", 281 | ], 282 | output="screen", 283 | ) 284 | 285 | load_joint_state_controller = ExecuteProcess( 286 | cmd=[ 287 | "ros2", 288 | "control", 289 | "load_controller", 290 | "--set-state", 291 | controller_run_state, 292 | "joint_state_broadcaster", 293 | "-c", 294 | namespace + "/controller_manager", 295 | ], 296 | output="screen", 297 | ) 298 | 299 | load_arm_trajectory_controller = ExecuteProcess( 300 | cmd=[ 301 | "ros2", 302 | "control", 303 | "load_controller", 304 | "--set-state", 305 | controller_run_state, 306 | "arm_controller", 307 | "-c", 308 | namespace + "/controller_manager", 309 | ], 310 | output="screen", 311 | ) 312 | message = """ { 313 | 'header': { 314 | 'stamp': { 315 | 'sec': 0, 316 | 'nanosec': 0 317 | }, 318 | 'frame_id': '' 319 | }, 320 | 'joint_names': [ 321 | 'shoulder_pan_joint', 322 | 'shoulder_lift_joint', 323 | 'elbow_joint', 324 | 'wrist_1_joint', 325 | 'wrist_2_joint', 326 | 'wrist_3_joint' 327 | ], 328 | 'points': [ 329 | { 330 | 'positions': [0.0, -0.97, 2.0, -2.56, -1.55, 0.0], 331 | 'velocities': [], 332 | 'accelerations': [], 333 | 'effort': [], 334 | 'time_from_start': { 335 | 'sec': 1, 336 | 'nanosec': 0 337 | } 338 | } 339 | ] 340 | }""" 341 | 342 | # Set initial joint position for robot. This step is not needed for Humble 343 | # In Humble, initial positions are taken from initial_positions.yaml and set by ros2 control plugin 344 | set_initial_pose = ExecuteProcess( 345 | cmd=[ 346 | "ros2", 347 | "topic", 348 | "pub", 349 | "--once", 350 | "/" + robot_name + "/arm_controller/joint_trajectory", 351 | "trajectory_msgs/msg/JointTrajectory", 352 | message, 353 | ], 354 | output="screen", 355 | ) 356 | 357 | if previous_final_action is not None: 358 | spawn_entity = RegisterEventHandler( 359 | event_handler=OnProcessExit( 360 | target_action=previous_final_action, 361 | on_exit=[robot_spawn_entity], 362 | ) 363 | ) 364 | else: 365 | spawn_entity = robot_spawn_entity 366 | 367 | state_controller_event = RegisterEventHandler( 368 | event_handler=OnProcessExit( 369 | target_action=robot_spawn_entity, 370 | on_exit=[load_joint_state_controller], 371 | ) 372 | ) 373 | arm_controller_event = RegisterEventHandler( 374 | event_handler=OnProcessExit( 375 | target_action=load_joint_state_controller, 376 | on_exit=[load_arm_trajectory_controller], 377 | ) 378 | ) 379 | 380 | set_initial_pose_event = RegisterEventHandler( 381 | event_handler=OnProcessExit( 382 | target_action=load_arm_trajectory_controller, 383 | on_exit=[set_initial_pose], 384 | ) 385 | ) 386 | 387 | ld.add_action(robot_state_publisher) 388 | ld.add_action(robot_move_group_node) 389 | ld.add_action(spawn_entity) 390 | ld.add_action(state_controller_event) 391 | ld.add_action(arm_controller_event) 392 | ld.add_action(set_initial_pose_event) 393 | 394 | return load_arm_trajectory_controller 395 | 396 | 397 | def load_file(package_path, file_path): 398 | 399 | absolute_file_path = os.path.join(package_path, file_path) 400 | try: 401 | with open(absolute_file_path, "r") as file: 402 | return file.read() 403 | except EnvironmentError: 404 | return None 405 | 406 | def load_yaml(package_path, file_path): 407 | 408 | absolute_file_path = os.path.join(package_path, file_path) 409 | try: 410 | with open(absolute_file_path, "r") as file: 411 | return yaml.safe_load(file) 412 | except EnvironmentError: 413 | return None 414 | -------------------------------------------------------------------------------- /urdf/ur/ur5/ur_ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 20 | 21 | 22 | 23 | 24 | gazebo_ros2_control/GazeboSystem 25 | 26 | 27 | ign_ros2_control/IgnitionSystem 28 | 29 | 30 | fake_components/GenericSystem 31 | ${fake_sensor_commands} 32 | 0.0 33 | 34 | 35 | ur_robot_driver/URPositionHardwareInterface 36 | ${robot_ip} 37 | ${script_filename} 38 | ${output_recipe_filename} 39 | ${input_recipe_filename} 40 | ${headless_mode} 41 | ${reverse_port} 42 | ${script_sender_port} 43 | "${tf_prefix}" 44 | 0 45 | 2000 46 | 0.03 47 | ${use_tool_communication} 48 | ${hash_kinematics} 49 | ${tool_voltage} 50 | ${tool_parity} 51 | ${tool_baud_rate} 52 | ${tool_stop_bits} 53 | ${tool_rx_idle_chars} 54 | ${tool_tx_idle_chars} 55 | ${tool_device_name} 56 | ${tool_tcp_port} 57 | 58 | 59 | 60 | 61 | {-2*pi} 62 | {2*pi} 63 | 64 | 65 | -3.15 66 | 3.15 67 | 68 | 69 | 70 | ${initial_positions['shoulder_pan_joint']} 71 | 72 | 73 | 74 | 75 | 76 | 77 | {-2*pi} 78 | {2*pi} 79 | 80 | 81 | -3.15 82 | 3.15 83 | 84 | 85 | 86 | ${initial_positions['shoulder_lift_joint']} 87 | 88 | 89 | 90 | 91 | 92 | 93 | {-pi} 94 | {pi} 95 | 96 | 97 | -3.15 98 | 3.15 99 | 100 | 101 | 102 | ${initial_positions['elbow_joint']} 103 | 104 | 105 | 106 | 107 | 108 | 109 | {-2*pi} 110 | {2*pi} 111 | 112 | 113 | -3.2 114 | 3.2 115 | 116 | 117 | 118 | ${initial_positions['wrist_1_joint']} 119 | 120 | 121 | 122 | 123 | 124 | 125 | {-2*pi} 126 | {2*pi} 127 | 128 | 129 | -3.2 130 | 3.2 131 | 132 | 133 | 134 | ${initial_positions['wrist_2_joint']} 135 | 136 | 137 | 138 | 139 | 140 | 141 | {-2*pi} 142 | {2*pi} 143 | 144 | 145 | -3.2 146 | 3.2 147 | 148 | 149 | 150 | ${initial_positions['wrist_3_joint']} 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 0 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 1 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | -------------------------------------------------------------------------------- /urdf/ur/inc/ur_common.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | -------------------------------------------------------------------------------- /urdf/ur/ur5/ur_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 52 | 53 | 54 | 55 | 56 | 90 | 91 | 92 | 98 | 99 | 100 | 101 | 102 | 103 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 100000.0 257 | 100000.0 258 | 259 | 260 | 261 | 262 | 0.003 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 10 378 | 10 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | --------------------------------------------------------------------------------