From 6449dccd6066dfc96a0c31e13c0928a88310accf Mon Sep 17 00:00:00 2001 From: Niko Date: Fri, 22 Mar 2024 16:51:46 +0100 Subject: [PATCH] ur_robotiq_control.launch.py and ur_robotiq_moveit.launch.py Working --- src/Universal_Robots_ROS2_Description | 2 +- ...launch.py => ur_robotiq_control.launch.py} | 108 +++++++------- .../urdf/ur_robotiq.urdf.xacro | 10 +- .../config/initial_positions.yaml | 14 +- .../config/kinematics.yaml | 14 +- ...ntrollers.yaml => moveit_controllers.yaml} | 8 +- .../config/ompl_planning.yaml | 116 ++++++++++++++- .../config/ompl_planning_old.yaml | 70 ++++++++++ .../launch/ur_robotiq_moveit.launch.py | 132 ++++++++++++------ .../rviz/view_robot.rviz | 2 +- ...{ur_robotiq.srdf => ur_robotiq.srdf.xacro} | 4 +- .../srdf/ur_robotiq_macro.srdf.xacro | 60 ++++++-- 12 files changed, 389 insertions(+), 151 deletions(-) rename src/ur_robotiq_description/launch/{ur_robotiq_controller.launch.py => ur_robotiq_control.launch.py} (91%) rename src/ur_robotiq_moveit_config/config/{controllers.yaml => moveit_controllers.yaml} (85%) create mode 100644 src/ur_robotiq_moveit_config/config/ompl_planning_old.yaml rename src/ur_robotiq_moveit_config/srdf/{ur_robotiq.srdf => ur_robotiq.srdf.xacro} (71%) diff --git a/src/Universal_Robots_ROS2_Description b/src/Universal_Robots_ROS2_Description index c1c8ac9..2e178fa 160000 --- a/src/Universal_Robots_ROS2_Description +++ b/src/Universal_Robots_ROS2_Description @@ -1 +1 @@ -Subproject commit c1c8ac92cd09e1b550837ad3566b427a957caf7c +Subproject commit 2e178fad2651b08c94324a8f96152cd1d78816dc diff --git a/src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py similarity index 91% rename from src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py rename to src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index b042e2a..61dcfad 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py +++ b/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=[ - robot_description, - update_rate_config_file, - ParameterFile(initial_joint_controllers, allow_substs=True), - ], + parameters=[robot_description, update_rate_config_file, initial_joint_controllers], 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=[ - robot_description, - update_rate_config_file, - ParameterFile(initial_joint_controllers, allow_substs=True), - ], + parameters=[robot_description, update_rate_config_file, initial_joint_controllers], 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 - def controller_spawner(name, active=True): - inactive_flags = ["--inactive"] if not active else [] - return Node( - package="controller_manager", - executable="spawner", - arguments=[ - name, - "--controller-manager", - "/controller_manager", - "--controller-manager-timeout", - controller_spawner_timeout, - ] - + inactive_flags, - ) + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) - controller_spawner_names = [ - "joint_state_broadcaster", - "io_and_status_controller", - "speed_scaling_state_broadcaster", - "force_torque_sensor_broadcaster", - ] - controller_spawner_inactive_names = ["forward_position_controller"] + io_and_status_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["io_and_status_controller", "-c", "/controller_manager"], + ) - controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ - controller_spawner(name, active=False) for name in controller_spawner_inactive_names - ] + speed_scaling_state_broadcaster_spawner = Node( + 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=[ - initial_joint_controller, - "-c", - "/controller_manager", - "--controller-manager-timeout", - controller_spawner_timeout, - ], + arguments=[initial_joint_controller, "-c", "/controller_manager"], condition=IfCondition(activate_joint_controller), ) initial_joint_controller_spawner_stopped = Node( package="controller_manager", executable="spawner", - arguments=[ - initial_joint_controller, - "-c", - "/controller_manager", - "--controller-manager-timeout", - controller_spawner_timeout, - "--inactive", - ], + arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"], 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 diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro index a774bfe..f4dc290 100644 --- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro +++ b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro @@ -1,5 +1,5 @@ - + @@ -99,18 +99,12 @@ - - - diff --git a/src/ur_robotiq_moveit_config/config/initial_positions.yaml b/src/ur_robotiq_moveit_config/config/initial_positions.yaml index 406c326..fa41209 100644 --- a/src/ur_robotiq_moveit_config/config/initial_positions.yaml +++ b/src/ur_robotiq_moveit_config/config/initial_positions.yaml @@ -1,7 +1,7 @@ -elbow_joint: 0 -shoulder_lift_joint: 0 -shoulder_pan_joint: 0 -wrist_1_joint: 0 -wrist_2_joint: 0 -wrist_3_joint: 0 -finger_joint: 0.695 \ No newline at end of file +shoulder_pan_joint: 0.0 +shoulder_lift_joint: -1.57 +elbow_joint: 0.0 +wrist_1_joint: -1.57 +wrist_2_joint: 0.0 +wrist_3_joint: 0.0 +finger_joint: 20.0 \ No newline at end of file diff --git a/src/ur_robotiq_moveit_config/config/kinematics.yaml b/src/ur_robotiq_moveit_config/config/kinematics.yaml index d7ab8e5..e6f6f74 100644 --- a/src/ur_robotiq_moveit_config/config/kinematics.yaml +++ b/src/ur_robotiq_moveit_config/config/kinematics.yaml @@ -1,12 +1,4 @@ ur_manipulator: - ros__parameters: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - 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 \ No newline at end of file + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 diff --git a/src/ur_robotiq_moveit_config/config/controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml similarity index 85% rename from src/ur_robotiq_moveit_config/config/controllers.yaml rename to src/ur_robotiq_moveit_config/config/moveit_controllers.yaml index 1cbefce..eecfc3f 100644 --- a/src/ur_robotiq_moveit_config/config/controllers.yaml +++ b/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 - type: GripperActionController + action_ns: gripper_cmd + type: GripperCommand default: true joints: - finger_joint - use_effort_interface: true - use_speed_interface: true + diff --git a/src/ur_robotiq_moveit_config/config/ompl_planning.yaml b/src/ur_robotiq_moveit_config/config/ompl_planning.yaml index 4dcc7a7..94527e1 100644 --- a/src/ur_robotiq_moveit_config/config/ompl_planning.yaml +++ b/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 - #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) - longest_valid_segment_fraction: 0.01 + - FMTkConfigDefault + - BFMTkConfigDefault + - 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 + diff --git a/src/ur_robotiq_moveit_config/config/ompl_planning_old.yaml b/src/ur_robotiq_moveit_config/config/ompl_planning_old.yaml new file mode 100644 index 0000000..4dcc7a7 --- /dev/null +++ b/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 diff --git a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py index 99e3a88..0898888 100644 --- a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py +++ b/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( - "robot_name", - default_value="ur", - description="Name of the Universal Robot" + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Type/series of used UR robot.", + default_value="ur3e", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], + ) ) - - use_fake_hardware_arg = DeclareLaunchArgument( + declared_arguments.append( + 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( - "use_sim_time", - default_value="false", - description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.", + declared_arguments.append( + DeclareLaunchArgument( + "warehouse_sqlite_path", + default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"), + description="Path where the warehouse database should be stored", + ) ) - prefix_arg = 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( + "use_sim_time", + default_value="false", + description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.", + ) + ) + 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) diff --git a/src/ur_robotiq_moveit_config/rviz/view_robot.rviz b/src/ur_robotiq_moveit_config/rviz/view_robot.rviz index 47e8f87..a3d3754 100644 --- a/src/ur_robotiq_moveit_config/rviz/view_robot.rviz +++ b/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 \ No newline at end of file diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf b/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf.xacro similarity index 71% rename from src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf rename to src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf.xacro index 79c1584..524eddf 100644 --- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf +++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf.xacro @@ -1,8 +1,8 @@ - + - + diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro index 77ba078..0f20db3 100644 --- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro +++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro @@ -1,5 +1,5 @@ - + @@ -11,6 +11,13 @@ + + + + + + + @@ -36,9 +43,25 @@ + + + + + + + + + + + + + + + + @@ -53,24 +76,45 @@ - + + + + - - - - - + + + + + + + - + + + + + + + + + + + + + + + + + \ No newline at end of file