ur_robotiq_control.launch.py

and ur_robotiq_moveit.launch.py
Working

src/Universal_Robots_ROS2_Description

@@ -1 +1 @@
-Subproject commit c1c8ac92cd09e1b550837ad3566b427a957caf7c
+Subproject commit 2e178fad2651b08c94324a8f96152cd1d78816dc

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -191,11 +191,7 @@ def launch_setup(context, *args, **kwargs):
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[
+        parameters=[robot_description, update_rate_config_file, initial_joint_controllers],
-            robot_description,
-            update_rate_config_file,
-            ParameterFile(initial_joint_controllers, allow_substs=True),
-        ],
         output="screen",
         condition=IfCondition(use_fake_hardware),
     )
@@ -203,11 +199,7 @@ def launch_setup(context, *args, **kwargs):
     ur_control_node = Node(
         package="ur_robot_driver",
         executable="ur_ros2_control_node",
-        parameters=[
+        parameters=[robot_description, update_rate_config_file, initial_joint_controllers],
-            robot_description,
-            update_rate_config_file,
-            ParameterFile(initial_joint_controllers, allow_substs=True),
-        ],
         output="screen",
         condition=UnlessCondition(use_fake_hardware),
     )
@@ -216,12 +208,14 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="spawner",
         arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
+        condition=UnlessCondition(use_fake_hardware),
     )
 
     robotiq_activation_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
+        condition=UnlessCondition(use_fake_hardware),
     )
 
     dashboard_client_node = Node(
@@ -249,13 +243,6 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-    urscript_interface = Node(
-        package="ur_robot_driver",
-        executable="urscript_interface",
-        parameters=[{"robot_ip": robot_ip}],
-        output="screen",
-    )
-
     controller_stopper_node = Node(
         package="ur_robot_driver",
         executable="controller_stopper_node",
@@ -272,6 +259,8 @@ def launch_setup(context, *args, **kwargs):
                     "force_torque_sensor_broadcaster",
                     "joint_state_broadcaster",
                     "speed_scaling_state_broadcaster",
+                    "robotiq_gripper_controller",
+                    "robotiq_activation_controller",
                 ]
             },
         ],
@@ -293,58 +282,55 @@ def launch_setup(context, *args, **kwargs):
         arguments=["-d", rviz_config_file],
     )
 
-    # Spawn controllers
+    joint_state_broadcaster_spawner = Node(
-    def controller_spawner(name, active=True):
+        package="controller_manager",
-        inactive_flags = ["--inactive"] if not active else []
+        executable="spawner",
-        return Node(
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-            package="controller_manager",
+    )
-            executable="spawner",
-            arguments=[
-                name,
-                "--controller-manager",
-                "/controller_manager",
-                "--controller-manager-timeout",
-                controller_spawner_timeout,
-            ]
-            + inactive_flags,
-        )
 
-    controller_spawner_names = [
+    io_and_status_controller_spawner = Node(
-        "joint_state_broadcaster",
+        package="controller_manager",
-        "io_and_status_controller",
+        executable="spawner",
-        "speed_scaling_state_broadcaster",
+        arguments=["io_and_status_controller", "-c", "/controller_manager"],
-        "force_torque_sensor_broadcaster",
+    )
-    ]
-    controller_spawner_inactive_names = ["forward_position_controller"]
 
-    controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
+    speed_scaling_state_broadcaster_spawner = Node(
-        controller_spawner(name, active=False) for name in controller_spawner_inactive_names
+        package="controller_manager",
-    ]
+        executable="spawner",
+        arguments=[
+            "speed_scaling_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    force_torque_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "force_torque_sensor_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    forward_position_controller_spawner_stopped = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["forward_position_controller", "-c", "/controller_manager", "--stopped"],
+    )
 
     # There may be other controllers of the joints, but this is the initially-started one
     initial_joint_controller_spawner_started = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=[
+        arguments=[initial_joint_controller, "-c", "/controller_manager"],
-            initial_joint_controller,
-            "-c",
-            "/controller_manager",
-            "--controller-manager-timeout",
-            controller_spawner_timeout,
-        ],
         condition=IfCondition(activate_joint_controller),
     )
     initial_joint_controller_spawner_stopped = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=[
+        arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"],
-            initial_joint_controller,
-            "-c",
-            "/controller_manager",
-            "--controller-manager-timeout",
-            controller_spawner_timeout,
-            "--inactive",
-        ],
         condition=UnlessCondition(activate_joint_controller),
     )
 
@@ -356,12 +342,16 @@ def launch_setup(context, *args, **kwargs):
         dashboard_client_node,
         tool_communication_node,
         controller_stopper_node,
-        urscript_interface,
         robot_state_publisher_node,
         rviz_node,
+        joint_state_broadcaster_spawner,
+        io_and_status_controller_spawner,
+        speed_scaling_state_broadcaster_spawner,
+        force_torque_sensor_broadcaster_spawner,
+        forward_position_controller_spawner_stopped,
         initial_joint_controller_spawner_stopped,
         initial_joint_controller_spawner_started,
-    ] + controller_spawners
+    ]
 
     return nodes_to_start
 

src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_manipulator">
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
 
 	<!-- parameters -->    
 	<xacro:arg name="name" default="ur_manipulator"/>
@@ -99,18 +99,12 @@
     <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
     </xacro:ur_robot>
 
-
    <xacro:robotiq_gripper name="$(arg tf_prefix)gripper"
     prefix="$(arg tf_prefix)"
-    parent="$(arg tf_prefix)ur_to_robotiq_link"
+    parent="$(arg tf_prefix)tool0"
     use_fake_hardware="$(arg use_fake_hardware)"
     >
     <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:robotiq_gripper>
 
-   <xacro:ur_to_robotiq prefix="$(arg tf_prefix)"
-     parent="$(arg tf_prefix)tool0"
-     child="$(arg tf_prefix)robotiq_base_link"
-     >
-   </xacro:ur_to_robotiq>
 </robot>

src/ur_robotiq_moveit_config/config/initial_positions.yaml

@@ -1,7 +1,7 @@
-elbow_joint: 0
+shoulder_pan_joint: 0.0
-shoulder_lift_joint: 0
+shoulder_lift_joint: -1.57
-shoulder_pan_joint: 0
+elbow_joint: 0.0
-wrist_1_joint: 0
+wrist_1_joint: -1.57
-wrist_2_joint: 0
+wrist_2_joint: 0.0
-wrist_3_joint: 0
+wrist_3_joint: 0.0
-finger_joint: 0.695
+finger_joint: 20.0

src/ur_robotiq_moveit_config/config/kinematics.yaml

@@ -1,12 +1,4 @@
 ur_manipulator:
-  ros__parameters:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-    kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
-    kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
-    kinematics_solver_timeout: 0.005
-    kinematics_solver_attempts: 3
-gripper:
-  ros__parameters:
-    kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-    kinematics_solver_search_resolution: 0.005
-    kinematics_solver_timeout: 0.005
-    kinematics_solver_attempts: 3

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -1,3 +1,4 @@
+
 controller_names:
   - scaled_joint_trajectory_controller
   - joint_trajectory_controller
@@ -30,11 +31,10 @@ joint_trajectory_controller:
     - wrist_3_joint
 
 robotiq_gripper_controller:
-  action_ns: position_controllers
+  action_ns: gripper_cmd
-  type: GripperActionController
+  type: GripperCommand
   default: true
   joints:
   - finger_joint
-  use_effort_interface: true
+
-  use_speed_interface: true
 

src/ur_robotiq_moveit_config/config/ompl_planning.yaml

@@ -46,12 +46,83 @@ planner_configs:
     init_temperature: 10e-6  # initial temperature. default: 10e-6
     frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
     frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
-    k_constant: 0.0  # value used to normalize expression. default: 0.0 set in setup()
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
   PRMkConfigDefault:
     type: geometric::PRM
     max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
   PRMstarkConfigDefault:
     type: geometric::PRMstar
+  FMTkConfigDefault:
+    type: geometric::FMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
+    nearest_k: 1  # use Knearest strategy. default: 1
+    cache_cc: 1  # use collision checking cache. default: 1
+    heuristics: 0  # activate cost to go heuristics. default: 0
+    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  BFMTkConfigDefault:
+    type: geometric::BFMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 1  # use the Knearest strategy. default: 1
+    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
+    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
+    heuristics: 1  # activates cost to go heuristics. default: 1
+    cache_cc: 1  # use the collision checking cache. default: 1
+    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  PDSTkConfigDefault:
+    type: geometric::PDST
+  STRIDEkConfigDefault:
+    type: geometric::STRIDE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    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
+    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
+    max_degree: 18  # max degree of a node in the GNAT. default: 12
+    min_degree: 12  # min degree of a node in the GNAT. default: 12
+    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
+    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
+    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
+  BiTRRTkConfigDefault:
+    type: geometric::BiTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
+    init_temperature: 100  # initial temperature. default: 100
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
+  LBTRRTkConfigDefault:
+    type: geometric::LBTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    epsilon: 0.4  # optimality approximation factor. default: 0.4
+  BiESTkConfigDefault:
+    type: geometric::BiEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ProjESTkConfigDefault:
+    type: geometric::ProjEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LazyPRMkConfigDefault:
+    type: geometric::LazyPRM
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  LazyPRMstarkConfigDefault:
+    type: geometric::LazyPRMstar
+  SPARSkConfigDefault:
+    type: geometric::SPARS
+    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
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 1000  # maximum consecutive failure limit. default: 1000
+  SPARStwokConfigDefault:
+    type: geometric::SPARStwo
+    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
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 5000  # maximum consecutive failure limit. default: 5000
+  TrajOptDefault:
+    type: geometric::TrajOpt
+
 ur_manipulator:
   planner_configs:
     - SBLkConfigDefault
@@ -65,6 +136,43 @@ ur_manipulator:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
-  ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
+    - FMTkConfigDefault
-  #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
+    - BFMTkConfigDefault
-  longest_valid_segment_fraction: 0.01
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault
+gripper:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault
+

src/ur_robotiq_moveit_config/config/ompl_planning_old.yaml

@@ -0,0 +1,70 @@
+planner_configs:
+  SBLkConfigDefault:
+    type: geometric::SBL
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRTkConfigDefault:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expression. default: 0.0 set in setup()
+  PRMkConfigDefault:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+ur_manipulator:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+  ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
+  #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
+  longest_valid_segment_fraction: 0.01

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -27,22 +27,24 @@ def load_yaml(package_name, file_path):
 def generate_launch_description():
 
     # Initialize Arguments
+    ur_type = LaunchConfiguration("ur_type")
     use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    safety_limits = LaunchConfiguration("safety_limits")
+    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
+    safety_k_position = LaunchConfiguration("safety_k_position")
     # General arguments
-    robot_name = LaunchConfiguration("robot_name")
+    warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
     prefix = LaunchConfiguration("prefix")
     use_sim_time = LaunchConfiguration("use_sim_time")
     launch_rviz = LaunchConfiguration("launch_rviz")
     launch_servo = LaunchConfiguration("launch_servo")
-    ur_type = "ur3e"
 
     # File and package names
     ur_description_package = "ur_description"
-    ur_moveit_config_package = "ur_moveit_config"
     ur_robotiq_description_package = "ur_robotiq_description"
     ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
     moveit_config_package = "ur_robotiq_moveit_config"
-    moveit_config_file = "ur_robotiq_macro.srdf.xacro"
+    moveit_config_file = "ur_robotiq.srdf.xacro"
 
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
@@ -65,9 +67,6 @@ def generate_launch_description():
             " ",
             "robot_ip:=xxx.yyy.zzz.www",
             " ",
-            "use_fake_hardware:=",
-            use_fake_hardware,
-            " ",
             "joint_limit_params:=",
             joint_limit_params,
             " ",
@@ -80,14 +79,17 @@ def generate_launch_description():
             "visual_params:=",
             visual_params,
             " ",
-            "safety_limits:=true",
+            "safety_limits:=",
+            safety_limits,
             " ",
-            "safety_pos_margin:=0.15",
+            "safety_pos_margin:=",
+            safety_pos_margin,
             " ",
-            "safety_k_position:=20",
+            "safety_k_position:=",
+            safety_k_position,
             " ",
             "name:=",
-            robot_name,
+            "ur",
             " ",
             "ur_type:=",
             ur_type,
@@ -114,7 +116,7 @@ def generate_launch_description():
             PathJoinSubstitution([FindPackageShare(moveit_config_package), "srdf", moveit_config_file]),
             " ",
             "name:=",
-            robot_name,
+            "ur",
             " ",
             "prefix:=",
             prefix,
@@ -124,8 +126,8 @@ def generate_launch_description():
 
     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
 
-    robot_description_kinematics = load_yaml(moveit_config_package, "config/kinematics.yaml")
+    robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml")
-
+    robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics_params}
     # robot_description_planning = {
     #     "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml")
     # }
@@ -142,10 +144,8 @@ def generate_launch_description():
     ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
-    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
-
     # Trajectory Execution Configuration
-    controllers_yaml = load_yaml(moveit_config_package, "config/controllers.yaml")
+    controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
     # the scaled_joint_trajectory_controller does not work on fake hardware
     change_controllers = use_fake_hardware
     if change_controllers == "true":
@@ -170,6 +170,12 @@ def generate_launch_description():
         "publish_state_updates": True,
         "publish_transforms_updates": True,
     }
+
+    warehouse_ros_config = {
+        "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
+        "warehouse_host": warehouse_sqlite_path,
+    }
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package="moveit_ros_move_group",
@@ -185,6 +191,7 @@ def generate_launch_description():
             moveit_controllers,
             planning_scene_monitor_parameters,
             {"use_sim_time": use_sim_time},
+            warehouse_ros_config,
         ],
     )
 
@@ -205,6 +212,7 @@ def generate_launch_description():
             ompl_planning_pipeline_config,
             robot_description_kinematics,
             # robot_description_planning,
+            warehouse_ros_config,
         ],
     )
 
@@ -225,44 +233,76 @@ def generate_launch_description():
     )
 
     # Declare the Launch arguments
+    declared_arguments = []
     # UR specific arguments
-    robot_name_arg = DeclareLaunchArgument(
+    declared_arguments.append(
-        "robot_name",
+        DeclareLaunchArgument(
-        default_value="ur",
+            "ur_type",
-        description="Name of the Universal Robot"
+            description="Type/series of used UR robot.",
+            default_value="ur3e",
+            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
+        )
     )
-
+    declared_arguments.append(
-    use_fake_hardware_arg = DeclareLaunchArgument(
+        DeclareLaunchArgument(
             "use_fake_hardware",
-            default_value="true",
+            default_value="false",
             description="Indicate whether robot is running with fake hardware mirroring command to its states.",
         )
-
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_limits",
+            default_value="true",
+            description="Enables the safety limits controller if true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_pos_margin",
+            default_value="0.15",
+            description="The margin to lower and upper limits in the safety controller.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_k_position",
+            default_value="20",
+            description="k-position factor in the safety controller.",
+        )
+    )
     # General arguments
-    use_sim_time_arg = DeclareLaunchArgument(
+    declared_arguments.append(
-        "use_sim_time",
+        DeclareLaunchArgument(
-        default_value="false",
+            "warehouse_sqlite_path",
-        description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
+            default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
+            description="Path where the warehouse database should be stored",
+        )
     )
-    prefix_arg = DeclareLaunchArgument(
+    declared_arguments.append(
-        "prefix",
+        DeclareLaunchArgument(
-        default_value='""',
+            "use_sim_time",
-        description="Prefix of the joint names, useful for \
+            default_value="false",
-    multi-robot setup. If changed than also joint names in the controllers' configuration \
+            description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
-    have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value='""',
+            description="Prefix of the joint names, useful for \
+        multi-robot setup. If changed than also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
     )
-    launch_rviz_arg = DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
-    launch_servo_arg = DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
 
-    launch_args = [
-        robot_name_arg,
-        prefix_arg,
-        use_fake_hardware_arg,
-        use_sim_time_arg,
-        launch_rviz_arg,
-        launch_servo_arg
-    ]
     # servo_node,
     nodes_to_start = [move_group_node, rviz_node, servo_node]
 
-    return LaunchDescription(launch_args + nodes_to_start)
+    return LaunchDescription(declared_arguments + nodes_to_start)

src/ur_robotiq_moveit_config/rviz/view_robot.rviz

@@ -308,4 +308,4 @@ Window Geometry:
     collapsed: false
   Width: 2560
   X: 0
-  Y: 30
+  Y: 30

src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf.xacro

@@ -1,8 +1,8 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_robotiq_srdf">
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
 
   <!-- robot name parameter -->
-  <xacro:arg name="name" default="ur_manipulator"/>
+  <xacro:arg name="name" default="ur"/>
   <!-- parameters -->
   <xacro:arg name="prefix" default="" />
 

src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro">
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
   <xacro:macro name="ur_robotiq_srdf" params="name prefix">
     <!--GROUPS - Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS - When a link is specified, the parent joint of that link (if it exists) is automatically included-->
@@ -11,6 +11,13 @@
     </group>
     <group name="${prefix}gripper">
         <joint name="${prefix}finger_joint"/>
+        <joint name="${prefix}left_outer_finger_joint" />
+        <joint name="${prefix}left_inner_knuckle_joint" />
+        <joint name="${prefix}left_inner_finger_joint" />
+        <joint name="${prefix}right_outer_knuckle_joint" />
+        <joint name="${prefix}right_outer_finger_joint" />
+        <joint name="${prefix}right_inner_knuckle_joint" />
+        <joint name="${prefix}right_inner_finger_joint" />
     </group>
     <!--GROUP STATES - Purpose - Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
@@ -36,9 +43,25 @@
       <joint name="${prefix}wrist_1_joint" value="-1.2" />
       <joint name="${prefix}wrist_2_joint" value="-1.6" />
       <joint name="${prefix}wrist_3_joint" value="-0.11" />
+    </group_state>
+        <group_state name="open" group="${prefix}gripper">
+        <joint name="${prefix}finger_joint" value="0" />
+    </group_state>
+    <group_state name="closed" group="${prefix}gripper">
+        <joint name="${prefix}finger_joint" value="0.698132" />
     </group_state>
     <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
+    <end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator" />
     <!--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)-->
+    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link" />
+    <!--PASSIVE JOINT - Purpose - this element is used to mark joints that are not actuated-->
+    <passive_joint name="${prefix}left_outer_finger_joint" />
+    <passive_joint name="${prefix}left_inner_knuckle_joint" />
+    <passive_joint name="${prefix}left_inner_finger_joint" />
+    <passive_joint name="${prefix}right_outer_knuckle_joint" />
+    <passive_joint name="${prefix}right_outer_finger_joint" />
+    <passive_joint name="${prefix}right_inner_knuckle_joint" />
+    <passive_joint name="${prefix}right_inner_finger_joint" />
     <!--DISABLE COLLISIONS - By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <!--UR-Robot-->
     <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
@@ -53,24 +76,45 @@
     <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never" />
     <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
     <!--Robotiq-Gripper-->
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+
+    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="left_outer_knuckle" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="right_outer_knuckle" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_outer_finger" reason="Adjacent"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
+
+    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
+
     <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
     <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_finger" reason="Adjacent"/>
     <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_finger" reason="Adjacent"/>
     <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
     <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
+
+    <!-- Weird Bug-->
+    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
+
+    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
+
+    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
   </xacro:macro>
 </robot>