From 65e5eaf0d8e7b00f92b8e656b6f48308827aad7f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 20 Jan 2021 21:46:16 +0100 Subject: [PATCH 01/28] Recreate ur10_moveit_config package using current setup-assistant This commit makes the moveit_config work with the description from melodic-devel-staging. --- ur10_moveit_config/.setup_assistant | 7 +- ur10_moveit_config/config/chomp_planning.yaml | 18 + ur10_moveit_config/config/controllers.yaml | 11 - .../config/fake_controllers.yaml | 5 +- ur10_moveit_config/config/joint_limits.yaml | 40 +- ur10_moveit_config/config/ompl_planning.yaml | 165 +++-- .../config/ros_controllers.yaml | 34 + ur10_moveit_config/config/ur10.srdf | 17 +- .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 6 +- ur10_moveit_config/launch/demo.launch | 51 +- ur10_moveit_config/launch/move_group.launch | 51 +- ur10_moveit_config/launch/moveit.rviz | 615 +++++++++++++++--- ur10_moveit_config/launch/moveit_rviz.launch | 11 +- .../launch/ompl_planning_pipeline.launch.xml | 2 +- .../launch/planning_context.launch | 11 +- .../launch/planning_pipeline.launch.xml | 4 +- .../launch/ros_controllers.launch | 11 + .../launch/setup_assistant.launch | 8 +- .../ur10_moveit_controller_manager.launch.xml | 2 +- .../ur10_moveit_planning_execution.launch | 6 +- ...robot_moveit_controller_manager.launch.xml | 10 + ...r10_robot_moveit_sensor_manager.launch.xml | 3 + ur10_moveit_config/launch/warehouse.launch | 6 +- .../launch/warehouse_settings.launch.xml | 9 +- ur10_moveit_config/package.xml | 1 + .../tests/moveit_planning_execution.xml | 7 +- 27 files changed, 866 insertions(+), 265 deletions(-) create mode 100644 ur10_moveit_config/config/chomp_planning.yaml delete mode 100644 ur10_moveit_config/config/controllers.yaml create mode 100644 ur10_moveit_config/config/ros_controllers.yaml create mode 100644 ur10_moveit_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 ur10_moveit_config/launch/ros_controllers.launch create mode 100644 ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml create mode 100644 ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml diff --git a/ur10_moveit_config/.setup_assistant b/ur10_moveit_config/.setup_assistant index 9129e624c..a613a4fcc 100644 --- a/ur10_moveit_config/.setup_assistant +++ b/ur10_moveit_config/.setup_assistant @@ -1,8 +1,11 @@ moveit_setup_assistant_config: URDF: package: ur_description - relative_path: urdf/ur10_robot.urdf.xacro + relative_path: urdf/ur10.xacro + xacro_args: "" SRDF: relative_path: config/ur10.srdf CONFIG: - generated_timestamp: 1413893323 \ No newline at end of file + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur10_moveit_config/config/chomp_planning.yaml b/ur10_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur10_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur10_moveit_config/config/controllers.yaml b/ur10_moveit_config/config/controllers.yaml deleted file mode 100644 index bf2252cb2..000000000 --- a/ur10_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur10_moveit_config/config/fake_controllers.yaml b/ur10_moveit_config/config/fake_controllers.yaml index 5e24f9451..e328e94d4 100644 --- a/ur10_moveit_config/config/fake_controllers.yaml +++ b/ur10_moveit_config/config/fake_controllers.yaml @@ -9,4 +9,7 @@ controller_list: - wrist_3_joint - name: fake_endeffector_controller joints: - [] \ No newline at end of file + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur10_moveit_config/config/joint_limits.yaml b/ur10_moveit_config/config/joint_limits.yaml index 51ce612f8..633b7bbce 100644 --- a/ur10_moveit_config/config/joint_limits.yaml +++ b/ur10_moveit_config/config/joint_limits.yaml @@ -2,33 +2,33 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - shoulder_pan_joint: + elbow_joint: has_velocity_limits: true - max_velocity: 2.16 - has_acceleration_limits: true - max_acceleration: 2.16 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 2.16 - has_acceleration_limits: true - max_acceleration: 2.16 - elbow_joint: + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 \ No newline at end of file + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur10_moveit_config/config/ompl_planning.yaml b/ur10_moveit_config/config/ompl_planning.yaml index b58b2d655..acbf6bd9b 100644 --- a/ur10_moveit_config/config/ompl_planning.yaml +++ b/ur10_moveit_config/config/ompl_planning.yaml @@ -1,42 +1,42 @@ planner_configs: - SBLkConfigDefault: + SBL: type: geometric::SBL range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: + EST: 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: + LBKPIECE: 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: + BKPIECE: 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: + KPIECE: 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 + 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: + RRT: 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: + RRTConnect: type: geometric::RRTConnect range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: + RRTstar: 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: + TRRT: 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 @@ -44,40 +44,129 @@ planner_configs: 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() + 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 expresssion. default: 0.0 set in setup() - PRMkConfigDefault: + PRM: type: geometric::PRM max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: + PRMstar: type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 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 + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo endeffector: planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur10_moveit_config/config/ros_controllers.yaml b/ur10_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..f0fd46f7b --- /dev/null +++ b/ur10_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur10_moveit_config/config/ur10.srdf b/ur10_moveit_config/config/ur10.srdf index dc3b1b817..48ae543d2 100644 --- a/ur10_moveit_config/config/ur10.srdf +++ b/ur10_moveit_config/config/ur10.srdf @@ -3,17 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - + - + @@ -33,16 +33,13 @@ - + - - - - - - + + + diff --git a/ur10_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur10_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..c8b77639a --- /dev/null +++ b/ur10_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur10_moveit_config/launch/default_warehouse_db.launch b/ur10_moveit_config/launch/default_warehouse_db.launch index 23cc824b6..b9fb81cb9 100644 --- a/ur10_moveit_config/launch/default_warehouse_db.launch +++ b/ur10_moveit_config/launch/default_warehouse_db.launch @@ -1,10 +1,12 @@ + + - + - + diff --git a/ur10_moveit_config/launch/demo.launch b/ur10_moveit_config/launch/demo.launch index c3c21e50b..6fe0ed6df 100644 --- a/ur10_moveit_config/launch/demo.launch +++ b/ur10_moveit_config/launch/demo.launch @@ -1,46 +1,63 @@ + + + + + - - - - - - - + + + + + + - + - - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] - + - + - + + + - - + + - + + + diff --git a/ur10_moveit_config/launch/move_group.launch b/ur10_moveit_config/launch/move_group.launch index eb1ba0a6a..4b39f0815 100644 --- a/ur10_moveit_config/launch/move_group.launch +++ b/ur10_moveit_config/launch/move_group.launch @@ -1,14 +1,10 @@ - - - - - - + @@ -16,15 +12,40 @@ + + + + + + + + + + + + + + + - + @@ -36,7 +57,7 @@ - + @@ -47,19 +68,9 @@ + + - - diff --git a/ur10_moveit_config/launch/moveit.rviz b/ur10_moveit_config/launch/moveit.rviz index c7540069f..b431b2a03 100644 --- a/ur10_moveit_config/launch/moveit.rviz +++ b/ur10_moveit_config/launch/moveit.rviz @@ -1,31 +1,19 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 84 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 299 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: @@ -49,106 +37,314 @@ Visualization Manager: Value: true - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: Alpha: 1 Show Axes: false Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ee_link: + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - forearm_link: + l_gripper_l_finger_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - shoulder_link: + l_gripper_l_finger_tip_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - tool0: + l_gripper_motor_accelerometer_link: Alpha: 1 Show Axes: false Show Trail: false - upper_arm_link: + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - world: + r_gripper_r_finger_link: Alpha: 1 Show Axes: false Show Trail: false - wrist_1_link: + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_2_link: + r_shoulder_pan_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_3_link: + r_upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - Loop Animation: false + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.08 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15 + Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: manipulator + Planning Group: left_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -157,7 +353,7 @@ Visualization Manager: Planning Scene Topic: move_group/monitored_planning_scene Robot Description: robot_description Scene Geometry: - Scene Alpha: 0.9 + Scene Alpha: 1 Scene Color: 50; 230; 50 Scene Display Time: 0.2 Show Scene Geometry: true @@ -166,125 +362,328 @@ Visualization Manager: Scene Robot: Attached Body Color: 150; 50; 150 Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: Alpha: 1 Show Axes: false Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ee_link: + bl_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - forearm_link: + bl_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - shoulder_link: + bl_caster_rotation_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - tool0: + br_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false - upper_arm_link: + Value: true + br_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - world: + br_caster_rotation_link: Alpha: 1 Show Axes: false Show Trail: false - wrist_1_link: + Value: true + double_stereo_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_2_link: + fl_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_3_link: + fl_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 + Fixed Frame: base_link Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.82854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false + Class: rviz/XYOrbit + Distance: 2.9965 Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 + Height: 1337 + Help: + collapsed: false Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Motion Planning: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur10_moveit_config/launch/moveit_rviz.launch b/ur10_moveit_config/launch/moveit_rviz.launch index 8a9f8bd11..a4605c037 100644 --- a/ur10_moveit_config/launch/moveit_rviz.launch +++ b/ur10_moveit_config/launch/moveit_rviz.launch @@ -4,13 +4,12 @@ - - - - + + + + - + args="$(arg command_args)" output="screen"> diff --git a/ur10_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur10_moveit_config/launch/ompl_planning_pipeline.launch.xml index 5f139376b..8a7299643 100644 --- a/ur10_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur10_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - - - - - + - + @@ -23,6 +19,7 @@ + - + diff --git a/ur10_moveit_config/launch/planning_pipeline.launch.xml b/ur10_moveit_config/launch/planning_pipeline.launch.xml index 04c97c1ad..b77dec8c0 100644 --- a/ur10_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur10_moveit_config/launch/planning_pipeline.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/ur10_moveit_config/launch/ros_controllers.launch b/ur10_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..5546211f8 --- /dev/null +++ b/ur10_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur10_moveit_config/launch/setup_assistant.launch b/ur10_moveit_config/launch/setup_assistant.launch index 2cf377c0c..b376032c7 100644 --- a/ur10_moveit_config/launch/setup_assistant.launch +++ b/ur10_moveit_config/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -7,9 +7,9 @@ - diff --git a/ur10_moveit_config/launch/ur10_moveit_controller_manager.launch.xml b/ur10_moveit_config/launch/ur10_moveit_controller_manager.launch.xml index 364b640ca..6ec589fab 100644 --- a/ur10_moveit_config/launch/ur10_moveit_controller_manager.launch.xml +++ b/ur10_moveit_config/launch/ur10_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch b/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch index 553e0775c..596ecb835 100644 --- a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch +++ b/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - + - diff --git a/ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml b/ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..252f650c3 --- /dev/null +++ b/ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml b/ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur10_moveit_config/launch/warehouse.launch b/ur10_moveit_config/launch/warehouse.launch index 8e4efaa80..2644d1d66 100644 --- a/ur10_moveit_config/launch/warehouse.launch +++ b/ur10_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - + - + diff --git a/ur10_moveit_config/launch/warehouse_settings.launch.xml b/ur10_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur10_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur10_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur10_moveit_config/package.xml b/ur10_moveit_config/package.xml index 443544c14..cc1ab542a 100644 --- a/ur10_moveit_config/package.xml +++ b/ur10_moveit_config/package.xml @@ -24,6 +24,7 @@ moveit_simple_controller_manager joint_state_publisher robot_state_publisher + tf2_ros xacro ur_description diff --git a/ur10_moveit_config/tests/moveit_planning_execution.xml b/ur10_moveit_config/tests/moveit_planning_execution.xml index 57cf9ff80..0d5f8c672 100644 --- a/ur10_moveit_config/tests/moveit_planning_execution.xml +++ b/ur10_moveit_config/tests/moveit_planning_execution.xml @@ -2,14 +2,13 @@ - - + - - + + From df0b44fbc0d32ef74a1c9269cdaec1e6fb6115d7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 13:52:07 +0100 Subject: [PATCH 02/28] Set default controller --- ur10_moveit_config/config/ros_controllers.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur10_moveit_config/config/ros_controllers.yaml b/ur10_moveit_config/config/ros_controllers.yaml index f0fd46f7b..0ecf90959 100644 --- a/ur10_moveit_config/config/ros_controllers.yaml +++ b/ur10_moveit_config/config/ros_controllers.yaml @@ -22,7 +22,7 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: -- name: "" +- name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory type: FollowJointTrajectory joints: From 222c2f02bbd8cc47f26f649e2f7896dc5a392686 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 11:43:29 +0100 Subject: [PATCH 03/28] Updated ur10e moveit config package --- universal_robots/package.xml | 2 +- ur10_e_moveit_config/.setup_assistant | 8 - ur10_e_moveit_config/CHANGELOG.rst | 10 - ur10_e_moveit_config/config/controllers.yaml | 11 - .../config/ompl_planning.yaml | 84 --- .../launch/default_warehouse_db.launch | 13 - ur10_e_moveit_config/launch/demo.launch | 46 -- ur10_e_moveit_config/launch/moveit.rviz | 290 -------- .../launch/moveit_rviz.launch | 16 - .../tests/moveit_planning_execution.xml | 16 - ur10e_moveit_config/.setup_assistant | 11 + ur10e_moveit_config/CHANGELOG.rst | 63 ++ .../CMakeLists.txt | 2 +- .../config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 5 +- .../config/joint_limits.yaml | 40 +- .../config/kinematics.yaml | 5 + ur10e_moveit_config/config/ompl_planning.yaml | 172 +++++ .../config/ros_controllers.yaml | 34 + .../config/ur10e.srdf | 17 +- .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 15 + ur10e_moveit_config/launch/demo.launch | 63 ++ .../fake_moveit_controller_manager.launch.xml | 2 +- .../launch/move_group.launch | 59 +- ur10e_moveit_config/launch/moveit.rviz | 689 ++++++++++++++++++ ur10e_moveit_config/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 4 +- .../launch/planning_context.launch | 17 +- .../launch/planning_pipeline.launch.xml | 6 +- .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 8 +- .../launch/sensor_manager.launch.xml | 8 +- .../launch/setup_assistant.launch | 8 +- .../launch/trajectory_execution.launch.xml | 4 +- ...ur10e_moveit_controller_manager.launch.xml | 2 +- .../ur10e_moveit_planning_execution.launch | 8 +- .../ur10e_moveit_sensor_manager.launch.xml | 0 ...robot_moveit_controller_manager.launch.xml | 10 + ...10e_robot_moveit_sensor_manager.launch.xml | 3 + .../launch/warehouse.launch | 8 +- .../launch/warehouse_settings.launch.xml | 9 +- .../package.xml | 7 +- .../tests/moveit_planning_execution.xml | 16 + 44 files changed, 1257 insertions(+), 598 deletions(-) delete mode 100644 ur10_e_moveit_config/.setup_assistant delete mode 100644 ur10_e_moveit_config/CHANGELOG.rst delete mode 100644 ur10_e_moveit_config/config/controllers.yaml delete mode 100644 ur10_e_moveit_config/config/ompl_planning.yaml delete mode 100644 ur10_e_moveit_config/launch/default_warehouse_db.launch delete mode 100644 ur10_e_moveit_config/launch/demo.launch delete mode 100644 ur10_e_moveit_config/launch/moveit.rviz delete mode 100644 ur10_e_moveit_config/launch/moveit_rviz.launch delete mode 100644 ur10_e_moveit_config/tests/moveit_planning_execution.xml create mode 100644 ur10e_moveit_config/.setup_assistant create mode 100644 ur10e_moveit_config/CHANGELOG.rst rename {ur10_e_moveit_config => ur10e_moveit_config}/CMakeLists.txt (92%) create mode 100644 ur10e_moveit_config/config/chomp_planning.yaml rename {ur10_e_moveit_config => ur10e_moveit_config}/config/fake_controllers.yaml (75%) rename {ur10_e_moveit_config => ur10e_moveit_config}/config/joint_limits.yaml (51%) rename {ur10_e_moveit_config => ur10e_moveit_config}/config/kinematics.yaml (50%) create mode 100644 ur10e_moveit_config/config/ompl_planning.yaml create mode 100644 ur10e_moveit_config/config/ros_controllers.yaml rename {ur10_e_moveit_config => ur10e_moveit_config}/config/ur10e.srdf (83%) create mode 100644 ur10e_moveit_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 ur10e_moveit_config/launch/default_warehouse_db.launch create mode 100644 ur10e_moveit_config/launch/demo.launch rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/fake_moveit_controller_manager.launch.xml (78%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/move_group.launch (54%) create mode 100644 ur10e_moveit_config/launch/moveit.rviz create mode 100644 ur10e_moveit_config/launch/moveit_rviz.launch rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/ompl_planning_pipeline.launch.xml (90%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/planning_context.launch (54%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/planning_pipeline.launch.xml (57%) create mode 100644 ur10e_moveit_config/launch/ros_controllers.launch rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/run_benchmark_ompl.launch (63%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/sensor_manager.launch.xml (70%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/setup_assistant.launch (60%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/trajectory_execution.launch.xml (83%) rename ur10_e_moveit_config/launch/ur10_e_moveit_controller_manager.launch.xml => ur10e_moveit_config/launch/ur10e_moveit_controller_manager.launch.xml (77%) rename ur10_e_moveit_config/launch/ur10_e_moveit_planning_execution.launch => ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch (65%) rename ur10_e_moveit_config/launch/ur10_e_moveit_sensor_manager.launch.xml => ur10e_moveit_config/launch/ur10e_moveit_sensor_manager.launch.xml (100%) create mode 100644 ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml create mode 100644 ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/warehouse.launch (67%) rename {ur10_e_moveit_config => ur10e_moveit_config}/launch/warehouse_settings.launch.xml (64%) rename {ur10_e_moveit_config => ur10e_moveit_config}/package.xml (94%) create mode 100644 ur10e_moveit_config/tests/moveit_planning_execution.xml diff --git a/universal_robots/package.xml b/universal_robots/package.xml index 5854f0a9b..2a4a0b082 100644 --- a/universal_robots/package.xml +++ b/universal_robots/package.xml @@ -20,7 +20,7 @@ catkin - ur10_e_moveit_config + ur10e_moveit_config ur10_moveit_config ur3_e_moveit_config ur3_moveit_config diff --git a/ur10_e_moveit_config/.setup_assistant b/ur10_e_moveit_config/.setup_assistant deleted file mode 100644 index fb157102a..000000000 --- a/ur10_e_moveit_config/.setup_assistant +++ /dev/null @@ -1,8 +0,0 @@ -moveit_setup_assistant_config: - URDF: - package: ur_e_description - relative_path: urdf/ur10e_robot.urdf.xacro - SRDF: - relative_path: config/ur10e.srdf - CONFIG: - generated_timestamp: 1413893323 diff --git a/ur10_e_moveit_config/CHANGELOG.rst b/ur10_e_moveit_config/CHANGELOG.rst deleted file mode 100644 index c8a9469fa..000000000 --- a/ur10_e_moveit_config/CHANGELOG.rst +++ /dev/null @@ -1,10 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ur10_e_moveit_config -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.5 (2019-04-05) ------------------- -* First release (of this package) -* Update maintainer listing: add Miguel (`#410 `_) -* UR-E Series (`#380 `_) -* Contributors: Dave Niewinski, gavanderhoorn diff --git a/ur10_e_moveit_config/config/controllers.yaml b/ur10_e_moveit_config/config/controllers.yaml deleted file mode 100644 index bf2252cb2..000000000 --- a/ur10_e_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur10_e_moveit_config/config/ompl_planning.yaml b/ur10_e_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index 191879b4e..000000000 --- a/ur10_e_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,84 +0,0 @@ -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 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 -manipulator: - default_planner_config: RRTConnectkConfigDefault - 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 -endeffector: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault diff --git a/ur10_e_moveit_config/launch/default_warehouse_db.launch b/ur10_e_moveit_config/launch/default_warehouse_db.launch deleted file mode 100644 index 8a207a25a..000000000 --- a/ur10_e_moveit_config/launch/default_warehouse_db.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/ur10_e_moveit_config/launch/demo.launch b/ur10_e_moveit_config/launch/demo.launch deleted file mode 100644 index f05986790..000000000 --- a/ur10_e_moveit_config/launch/demo.launch +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ur10_e_moveit_config/launch/moveit.rviz b/ur10_e_moveit_config/launch/moveit.rviz deleted file mode 100644 index c7540069f..000000000 --- a/ur10_e_moveit_config/launch/moveit.rviz +++ /dev/null @@ -1,290 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 75 - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 299 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ee_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.08 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15 - Joint Violation Color: 255; 0; 255 - Planning Group: manipulator - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.9 - Scene Color: 50; 230; 50 - Scene Display Time: 0.2 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ee_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.82854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 diff --git a/ur10_e_moveit_config/launch/moveit_rviz.launch b/ur10_e_moveit_config/launch/moveit_rviz.launch deleted file mode 100644 index 4859a41f4..000000000 --- a/ur10_e_moveit_config/launch/moveit_rviz.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/ur10_e_moveit_config/tests/moveit_planning_execution.xml b/ur10_e_moveit_config/tests/moveit_planning_execution.xml deleted file mode 100644 index 20e58cb05..000000000 --- a/ur10_e_moveit_config/tests/moveit_planning_execution.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/ur10e_moveit_config/.setup_assistant b/ur10e_moveit_config/.setup_assistant new file mode 100644 index 000000000..f2f266c98 --- /dev/null +++ b/ur10e_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: ur_description + relative_path: urdf/ur10e.xacro + xacro_args: "" + SRDF: + relative_path: config/ur10e.srdf + CONFIG: + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur10e_moveit_config/CHANGELOG.rst b/ur10e_moveit_config/CHANGELOG.rst new file mode 100644 index 000000000..86fac4e22 --- /dev/null +++ b/ur10e_moveit_config/CHANGELOG.rst @@ -0,0 +1,63 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ur10_moveit_config +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.5 (2019-04-05) +------------------ +* Update maintainer listing: add Miguel (`#410 `_) +* MoveGroupExecuteService is Deprecated by MoveIt! (`#391 `_) +* Update maintainer and author information. +* Add roslaunch tests (`#362 `_) +* Contributors: gavanderhoorn, Nadia Hammoudeh García, 薯片半价 + +1.2.1 (2018-01-06) +------------------ +* Reduce longest valid segment fraction to accomodate non-limited version of the UR5 (`#266 `_) +* Contributors: Scott Paulin + +1.2.0 (2017-08-04) +------------------ +* Fix Deprecated warning in MoveIt: parameter moved into namespace 'trajectory_execution' +* Contributors: Dave Coleman + +1.1.9 (2017-01-02) +------------------ +* use '--inorder' for jade+ xacro as well. +* make RViz load MoveIt display by default. +* Contributors: gavanderhoorn + +1.1.8 (2016-12-30) +------------------ +* all: update maintainers. +* Contributors: gavanderhoorn + +1.1.7 (2016-12-29) +------------------ +* Don't depend on moveit_plugins metapackage +* Fix xacro warnings in Jade +* Contributors: Dave Coleman, Jon Binney + +1.1.6 (2016-04-01) +------------------ +* add missing dependency for moveit_simple_controller_manager +* Merge branch 'indigo-devel' of github.com:ros-industrial/universal_robot into ur3_moveit_config +* apply latest setup assistant changes to ur5 and ur10 +* Adding comment explaining the choice of default planning algorithm +* Use RRTConnect by default for UR10 + Fixes bug `#193 `_ about slow planning on Indigo + LBKPIECE1 (the previous default) looks to be the wrong planning algorithm for the robot + See https://groups.google.com/forum/#!topic/moveit-users/M71T-GaUNgg +* crop ik solutions wrt joint_limits +* set planning time to 0 +* reduce planning attempts in moveit rviz plugin +* Contributors: Marco Esposito, ipa-fxm + +1.0.2 (2014-03-31) +------------------ + +1.0.1 (2014-03-31) +------------------ +* changes due to file renaming +* update moveit_configs: include ee_link and handle limited robot +* new moveit_configs for ur5 and ur10 +* Contributors: ipa-fxm diff --git a/ur10_e_moveit_config/CMakeLists.txt b/ur10e_moveit_config/CMakeLists.txt similarity index 92% rename from ur10_e_moveit_config/CMakeLists.txt rename to ur10e_moveit_config/CMakeLists.txt index 408778222..d665149d3 100644 --- a/ur10_e_moveit_config/CMakeLists.txt +++ b/ur10e_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ur10_e_moveit_config) +project(ur10e_moveit_config) find_package(catkin REQUIRED) diff --git a/ur10e_moveit_config/config/chomp_planning.yaml b/ur10e_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur10e_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur10_e_moveit_config/config/fake_controllers.yaml b/ur10e_moveit_config/config/fake_controllers.yaml similarity index 75% rename from ur10_e_moveit_config/config/fake_controllers.yaml rename to ur10e_moveit_config/config/fake_controllers.yaml index 5e24f9451..e328e94d4 100644 --- a/ur10_e_moveit_config/config/fake_controllers.yaml +++ b/ur10e_moveit_config/config/fake_controllers.yaml @@ -9,4 +9,7 @@ controller_list: - wrist_3_joint - name: fake_endeffector_controller joints: - [] \ No newline at end of file + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur10_e_moveit_config/config/joint_limits.yaml b/ur10e_moveit_config/config/joint_limits.yaml similarity index 51% rename from ur10_e_moveit_config/config/joint_limits.yaml rename to ur10e_moveit_config/config/joint_limits.yaml index 991719a66..633b7bbce 100644 --- a/ur10_e_moveit_config/config/joint_limits.yaml +++ b/ur10e_moveit_config/config/joint_limits.yaml @@ -2,33 +2,33 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - shoulder_pan_joint: + elbow_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 - elbow_joint: + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur10_e_moveit_config/config/kinematics.yaml b/ur10e_moveit_config/config/kinematics.yaml similarity index 50% rename from ur10_e_moveit_config/config/kinematics.yaml rename to ur10e_moveit_config/config/kinematics.yaml index 5d492ac1f..e37d336c3 100644 --- a/ur10_e_moveit_config/config/kinematics.yaml +++ b/ur10e_moveit_config/config/kinematics.yaml @@ -1,3 +1,8 @@ +#manipulator: +# kinematics_solver: ur_kinematics/UR10KinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 diff --git a/ur10e_moveit_config/config/ompl_planning.yaml b/ur10e_moveit_config/config/ompl_planning.yaml new file mode 100644 index 000000000..acbf6bd9b --- /dev/null +++ b/ur10e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,172 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + 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 + LBKPIECE: + 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 + BKPIECE: + 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 + KPIECE: + 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 + RRT: + 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 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + 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 + TRRT: + 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 expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 +manipulator: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +endeffector: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur10e_moveit_config/config/ros_controllers.yaml b/ur10e_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..0ecf90959 --- /dev/null +++ b/ur10e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "scaled_pos_joint_traj_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur10_e_moveit_config/config/ur10e.srdf b/ur10e_moveit_config/config/ur10e.srdf similarity index 83% rename from ur10_e_moveit_config/config/ur10e.srdf rename to ur10e_moveit_config/config/ur10e.srdf index ad686256f..77bccf0d3 100644 --- a/ur10_e_moveit_config/config/ur10e.srdf +++ b/ur10e_moveit_config/config/ur10e.srdf @@ -3,17 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - + - + @@ -33,16 +33,13 @@ - + - - - - - - + + + diff --git a/ur10e_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur10e_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..600f8a7e9 --- /dev/null +++ b/ur10e_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur10e_moveit_config/launch/default_warehouse_db.launch b/ur10e_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 000000000..bc6736272 --- /dev/null +++ b/ur10e_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/ur10e_moveit_config/launch/demo.launch b/ur10e_moveit_config/launch/demo.launch new file mode 100644 index 000000000..32ba509f0 --- /dev/null +++ b/ur10e_moveit_config/launch/demo.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur10_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur10e_moveit_config/launch/fake_moveit_controller_manager.launch.xml similarity index 78% rename from ur10_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml rename to ur10e_moveit_config/launch/fake_moveit_controller_manager.launch.xml index 1f3f4de92..ed8eb47d5 100644 --- a/ur10_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ b/ur10e_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -4,6 +4,6 @@ - + diff --git a/ur10_e_moveit_config/launch/move_group.launch b/ur10e_moveit_config/launch/move_group.launch similarity index 54% rename from ur10_e_moveit_config/launch/move_group.launch rename to ur10e_moveit_config/launch/move_group.launch index efeb0a045..0dc118e8e 100644 --- a/ur10_e_moveit_config/launch/move_group.launch +++ b/ur10e_moveit_config/launch/move_group.launch @@ -1,14 +1,10 @@ - - - - - - + @@ -16,27 +12,52 @@ + + + + + + + + + + + + + + + - - + + - + - + - - + + @@ -47,19 +68,9 @@ + + - - diff --git a/ur10e_moveit_config/launch/moveit.rviz b/ur10e_moveit_config/launch/moveit.rviz new file mode 100644 index 000000000..b431b2a03 --- /dev/null +++ b/ur10e_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur10e_moveit_config/launch/moveit_rviz.launch b/ur10e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..a4605c037 --- /dev/null +++ b/ur10e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur10_e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur10e_moveit_config/launch/ompl_planning_pipeline.launch.xml similarity index 90% rename from ur10_e_moveit_config/launch/ompl_planning_pipeline.launch.xml rename to ur10e_moveit_config/launch/ompl_planning_pipeline.launch.xml index 2a6b4c1a7..a52a7c101 100644 --- a/ur10_e_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur10e_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - + diff --git a/ur10_e_moveit_config/launch/planning_context.launch b/ur10e_moveit_config/launch/planning_context.launch similarity index 54% rename from ur10_e_moveit_config/launch/planning_context.launch rename to ur10e_moveit_config/launch/planning_context.launch index a87b942d1..a0121a8de 100644 --- a/ur10_e_moveit_config/launch/planning_context.launch +++ b/ur10e_moveit_config/launch/planning_context.launch @@ -1,28 +1,25 @@ - - - - - + - - + + - + - + + - + diff --git a/ur10_e_moveit_config/launch/planning_pipeline.launch.xml b/ur10e_moveit_config/launch/planning_pipeline.launch.xml similarity index 57% rename from ur10_e_moveit_config/launch/planning_pipeline.launch.xml rename to ur10e_moveit_config/launch/planning_pipeline.launch.xml index a0d6c2960..277b80706 100644 --- a/ur10_e_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur10e_moveit_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/ur10e_moveit_config/launch/ros_controllers.launch b/ur10e_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..ec48b6bd6 --- /dev/null +++ b/ur10e_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur10_e_moveit_config/launch/run_benchmark_ompl.launch b/ur10e_moveit_config/launch/run_benchmark_ompl.launch similarity index 63% rename from ur10_e_moveit_config/launch/run_benchmark_ompl.launch rename to ur10e_moveit_config/launch/run_benchmark_ompl.launch index 9c4615c8d..3553a913f 100644 --- a/ur10_e_moveit_config/launch/run_benchmark_ompl.launch +++ b/ur10e_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/ur10_e_moveit_config/launch/sensor_manager.launch.xml b/ur10e_moveit_config/launch/sensor_manager.launch.xml similarity index 70% rename from ur10_e_moveit_config/launch/sensor_manager.launch.xml rename to ur10e_moveit_config/launch/sensor_manager.launch.xml index 90cf60d85..7140a9f3f 100644 --- a/ur10_e_moveit_config/launch/sensor_manager.launch.xml +++ b/ur10e_moveit_config/launch/sensor_manager.launch.xml @@ -1,6 +1,6 @@ - + @@ -8,7 +8,7 @@ - - - + + + diff --git a/ur10_e_moveit_config/launch/setup_assistant.launch b/ur10e_moveit_config/launch/setup_assistant.launch similarity index 60% rename from ur10_e_moveit_config/launch/setup_assistant.launch rename to ur10e_moveit_config/launch/setup_assistant.launch index f35f15b87..6fc48197b 100644 --- a/ur10_e_moveit_config/launch/setup_assistant.launch +++ b/ur10e_moveit_config/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -7,9 +7,9 @@ - diff --git a/ur10_e_moveit_config/launch/trajectory_execution.launch.xml b/ur10e_moveit_config/launch/trajectory_execution.launch.xml similarity index 83% rename from ur10_e_moveit_config/launch/trajectory_execution.launch.xml rename to ur10e_moveit_config/launch/trajectory_execution.launch.xml index 2dfb6e8bf..3c25c0689 100644 --- a/ur10_e_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur10e_moveit_config/launch/trajectory_execution.launch.xml @@ -12,7 +12,7 @@ - - + + diff --git a/ur10_e_moveit_config/launch/ur10_e_moveit_controller_manager.launch.xml b/ur10e_moveit_config/launch/ur10e_moveit_controller_manager.launch.xml similarity index 77% rename from ur10_e_moveit_config/launch/ur10_e_moveit_controller_manager.launch.xml rename to ur10e_moveit_config/launch/ur10e_moveit_controller_manager.launch.xml index ac361c10a..114ae0a28 100644 --- a/ur10_e_moveit_config/launch/ur10_e_moveit_controller_manager.launch.xml +++ b/ur10e_moveit_config/launch/ur10e_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur10_e_moveit_config/launch/ur10_e_moveit_planning_execution.launch b/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch similarity index 65% rename from ur10_e_moveit_config/launch/ur10_e_moveit_planning_execution.launch rename to ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch index b701ce754..e7240f802 100644 --- a/ur10_e_moveit_config/launch/ur10_e_moveit_planning_execution.launch +++ b/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - + - - + diff --git a/ur10_e_moveit_config/launch/ur10_e_moveit_sensor_manager.launch.xml b/ur10e_moveit_config/launch/ur10e_moveit_sensor_manager.launch.xml similarity index 100% rename from ur10_e_moveit_config/launch/ur10_e_moveit_sensor_manager.launch.xml rename to ur10e_moveit_config/launch/ur10e_moveit_sensor_manager.launch.xml diff --git a/ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml b/ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..b4b5dd22b --- /dev/null +++ b/ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml b/ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur10_e_moveit_config/launch/warehouse.launch b/ur10e_moveit_config/launch/warehouse.launch similarity index 67% rename from ur10_e_moveit_config/launch/warehouse.launch rename to ur10e_moveit_config/launch/warehouse.launch index b0c8eeefe..c6548b5d4 100644 --- a/ur10_e_moveit_config/launch/warehouse.launch +++ b/ur10e_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - - + + - + diff --git a/ur10_e_moveit_config/launch/warehouse_settings.launch.xml b/ur10e_moveit_config/launch/warehouse_settings.launch.xml similarity index 64% rename from ur10_e_moveit_config/launch/warehouse_settings.launch.xml rename to ur10e_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur10_e_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur10e_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur10_e_moveit_config/package.xml b/ur10e_moveit_config/package.xml similarity index 94% rename from ur10_e_moveit_config/package.xml rename to ur10e_moveit_config/package.xml index bceb8039d..c1f63a751 100644 --- a/ur10_e_moveit_config/package.xml +++ b/ur10e_moveit_config/package.xml @@ -1,7 +1,7 @@ - ur10_e_moveit_config + ur10e_moveit_config 1.2.5 An automatically generated package with all the configuration and launch files for using the ur10e with the MoveIt Motion Planning Framework @@ -10,7 +10,7 @@ G.A. vd. Hoorn Miguel Prada Sarasola Nadia Hammoudeh Garcia - + BSD http://moveit.ros.org/ @@ -24,10 +24,11 @@ moveit_simple_controller_manager joint_state_publisher robot_state_publisher + tf2_ros xacro ur_description roslaunch catkin - + diff --git a/ur10e_moveit_config/tests/moveit_planning_execution.xml b/ur10e_moveit_config/tests/moveit_planning_execution.xml new file mode 100644 index 000000000..910165b8b --- /dev/null +++ b/ur10e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + From c5ab4de38be4015c8a01b28634d78e32b6660da2 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 12:00:11 +0100 Subject: [PATCH 04/28] Updated ur3 moveit config --- ur3_moveit_config/.setup_assistant | 7 +- ur3_moveit_config/config/chomp_planning.yaml | 18 + ur3_moveit_config/config/controllers.yaml | 11 - .../config/fake_controllers.yaml | 5 +- ur3_moveit_config/config/joint_limits.yaml | 40 +- ur3_moveit_config/config/kinematics.yaml | 2 +- ur3_moveit_config/config/ompl_planning.yaml | 165 +++-- ur3_moveit_config/config/ros_controllers.yaml | 34 + ur3_moveit_config/config/ur3.srdf | 16 +- .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 6 +- ur3_moveit_config/launch/demo.launch | 51 +- .../launch/joystick_control.launch | 17 - ur3_moveit_config/launch/move_group.launch | 48 +- ur3_moveit_config/launch/moveit.rviz | 615 +++++++++++++++--- ur3_moveit_config/launch/moveit_rviz.launch | 11 +- .../launch/ompl_planning_pipeline.launch.xml | 2 +- .../launch/planning_context.launch | 11 +- .../launch/planning_pipeline.launch.xml | 4 +- .../launch/ros_controllers.launch | 11 + .../launch/setup_assistant.launch | 8 +- .../ur3_moveit_controller_manager.launch.xml | 2 +- .../ur3_moveit_planning_execution.launch | 6 +- ...robot_moveit_controller_manager.launch.xml | 10 + ...ur3_robot_moveit_sensor_manager.launch.xml | 3 + ur3_moveit_config/launch/warehouse.launch | 6 +- .../launch/warehouse_settings.launch.xml | 9 +- ur3_moveit_config/package.xml | 1 + .../tests/moveit_planning_execution.xml | 8 +- 29 files changed, 865 insertions(+), 282 deletions(-) create mode 100644 ur3_moveit_config/config/chomp_planning.yaml delete mode 100644 ur3_moveit_config/config/controllers.yaml create mode 100644 ur3_moveit_config/config/ros_controllers.yaml create mode 100644 ur3_moveit_config/launch/chomp_planning_pipeline.launch.xml delete mode 100644 ur3_moveit_config/launch/joystick_control.launch create mode 100644 ur3_moveit_config/launch/ros_controllers.launch create mode 100644 ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml create mode 100644 ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml diff --git a/ur3_moveit_config/.setup_assistant b/ur3_moveit_config/.setup_assistant index c229d69bd..92ffcddc0 100644 --- a/ur3_moveit_config/.setup_assistant +++ b/ur3_moveit_config/.setup_assistant @@ -1,8 +1,11 @@ moveit_setup_assistant_config: URDF: package: ur_description - relative_path: urdf/ur3_robot.urdf.xacro + relative_path: urdf/ur3.xacro + xacro_args: "" SRDF: relative_path: config/ur3.srdf CONFIG: - generated_timestamp: 1438427891 \ No newline at end of file + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur3_moveit_config/config/chomp_planning.yaml b/ur3_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur3_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur3_moveit_config/config/controllers.yaml b/ur3_moveit_config/config/controllers.yaml deleted file mode 100644 index bf2252cb2..000000000 --- a/ur3_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur3_moveit_config/config/fake_controllers.yaml b/ur3_moveit_config/config/fake_controllers.yaml index 5e24f9451..e328e94d4 100644 --- a/ur3_moveit_config/config/fake_controllers.yaml +++ b/ur3_moveit_config/config/fake_controllers.yaml @@ -9,4 +9,7 @@ controller_list: - wrist_3_joint - name: fake_endeffector_controller joints: - [] \ No newline at end of file + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur3_moveit_config/config/joint_limits.yaml b/ur3_moveit_config/config/joint_limits.yaml index 60797d513..633b7bbce 100644 --- a/ur3_moveit_config/config/joint_limits.yaml +++ b/ur3_moveit_config/config/joint_limits.yaml @@ -2,33 +2,33 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - shoulder_pan_joint: + elbow_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 - elbow_joint: + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 \ No newline at end of file + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur3_moveit_config/config/kinematics.yaml b/ur3_moveit_config/config/kinematics.yaml index 7162d768c..e37d336c3 100644 --- a/ur3_moveit_config/config/kinematics.yaml +++ b/ur3_moveit_config/config/kinematics.yaml @@ -1,5 +1,5 @@ #manipulator: -# kinematics_solver: ur_kinematics/UR3KinematicsPlugin +# kinematics_solver: ur_kinematics/UR10KinematicsPlugin # kinematics_solver_search_resolution: 0.005 # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 diff --git a/ur3_moveit_config/config/ompl_planning.yaml b/ur3_moveit_config/config/ompl_planning.yaml index b58b2d655..acbf6bd9b 100644 --- a/ur3_moveit_config/config/ompl_planning.yaml +++ b/ur3_moveit_config/config/ompl_planning.yaml @@ -1,42 +1,42 @@ planner_configs: - SBLkConfigDefault: + SBL: type: geometric::SBL range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: + EST: 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: + LBKPIECE: 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: + BKPIECE: 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: + KPIECE: 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 + 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: + RRT: 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: + RRTConnect: type: geometric::RRTConnect range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: + RRTstar: 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: + TRRT: 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 @@ -44,40 +44,129 @@ planner_configs: 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() + 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 expresssion. default: 0.0 set in setup() - PRMkConfigDefault: + PRM: type: geometric::PRM max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: + PRMstar: type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 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 + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo endeffector: planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur3_moveit_config/config/ros_controllers.yaml b/ur3_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..0ecf90959 --- /dev/null +++ b/ur3_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "scaled_pos_joint_traj_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur3_moveit_config/config/ur3.srdf b/ur3_moveit_config/config/ur3.srdf index d1f117c06..5447d2f0f 100644 --- a/ur3_moveit_config/config/ur3.srdf +++ b/ur3_moveit_config/config/ur3.srdf @@ -3,17 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - + - + @@ -33,17 +33,15 @@ - + - - - - + + + - diff --git a/ur3_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur3_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..f22ea99c2 --- /dev/null +++ b/ur3_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur3_moveit_config/launch/default_warehouse_db.launch b/ur3_moveit_config/launch/default_warehouse_db.launch index 7b39a3ddc..5291dfc32 100644 --- a/ur3_moveit_config/launch/default_warehouse_db.launch +++ b/ur3_moveit_config/launch/default_warehouse_db.launch @@ -1,10 +1,12 @@ + + - + - + diff --git a/ur3_moveit_config/launch/demo.launch b/ur3_moveit_config/launch/demo.launch index 4d0ebee58..26d9aa40f 100644 --- a/ur3_moveit_config/launch/demo.launch +++ b/ur3_moveit_config/launch/demo.launch @@ -1,46 +1,63 @@ + + + + + - - - - - - - + + + + + + - + - - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] - + - + - + + + - - + + - + + + diff --git a/ur3_moveit_config/launch/joystick_control.launch b/ur3_moveit_config/launch/joystick_control.launch deleted file mode 100644 index f74173527..000000000 --- a/ur3_moveit_config/launch/joystick_control.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/ur3_moveit_config/launch/move_group.launch b/ur3_moveit_config/launch/move_group.launch index 625f436c8..c0229dbfd 100644 --- a/ur3_moveit_config/launch/move_group.launch +++ b/ur3_moveit_config/launch/move_group.launch @@ -1,15 +1,10 @@ - - - - - + value="gdb -x $(find ur3_moveit_config)/launch/gdb_settings.gdb --ex run --args" /> @@ -17,15 +12,40 @@ + + + + + + + + + + + + + + + - + @@ -48,19 +68,9 @@ + + - - diff --git a/ur3_moveit_config/launch/moveit.rviz b/ur3_moveit_config/launch/moveit.rviz index c7540069f..b431b2a03 100644 --- a/ur3_moveit_config/launch/moveit.rviz +++ b/ur3_moveit_config/launch/moveit.rviz @@ -1,31 +1,19 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 84 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 299 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: @@ -49,106 +37,314 @@ Visualization Manager: Value: true - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: Alpha: 1 Show Axes: false Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ee_link: + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - forearm_link: + l_gripper_l_finger_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - shoulder_link: + l_gripper_l_finger_tip_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - tool0: + l_gripper_motor_accelerometer_link: Alpha: 1 Show Axes: false Show Trail: false - upper_arm_link: + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - world: + r_gripper_r_finger_link: Alpha: 1 Show Axes: false Show Trail: false - wrist_1_link: + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_2_link: + r_shoulder_pan_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_3_link: + r_upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - Loop Animation: false + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.08 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15 + Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: manipulator + Planning Group: left_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -157,7 +353,7 @@ Visualization Manager: Planning Scene Topic: move_group/monitored_planning_scene Robot Description: robot_description Scene Geometry: - Scene Alpha: 0.9 + Scene Alpha: 1 Scene Color: 50; 230; 50 Scene Display Time: 0.2 Show Scene Geometry: true @@ -166,125 +362,328 @@ Visualization Manager: Scene Robot: Attached Body Color: 150; 50; 150 Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: Alpha: 1 Show Axes: false Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ee_link: + bl_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - forearm_link: + bl_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - shoulder_link: + bl_caster_rotation_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - tool0: + br_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false - upper_arm_link: + Value: true + br_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - world: + br_caster_rotation_link: Alpha: 1 Show Axes: false Show Trail: false - wrist_1_link: + Value: true + double_stereo_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_2_link: + fl_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_3_link: + fl_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 + Fixed Frame: base_link Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.82854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false + Class: rviz/XYOrbit + Distance: 2.9965 Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 + Height: 1337 + Help: + collapsed: false Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Motion Planning: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur3_moveit_config/launch/moveit_rviz.launch b/ur3_moveit_config/launch/moveit_rviz.launch index 8aa1b22d5..a4605c037 100644 --- a/ur3_moveit_config/launch/moveit_rviz.launch +++ b/ur3_moveit_config/launch/moveit_rviz.launch @@ -4,13 +4,12 @@ - - - - + + + + - + args="$(arg command_args)" output="screen"> diff --git a/ur3_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur3_moveit_config/launch/ompl_planning_pipeline.launch.xml index c67fdfc84..b41b24c01 100644 --- a/ur3_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur3_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - - - - - + - + @@ -23,6 +19,7 @@ + - + diff --git a/ur3_moveit_config/launch/planning_pipeline.launch.xml b/ur3_moveit_config/launch/planning_pipeline.launch.xml index ae6a311d1..527abc667 100644 --- a/ur3_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur3_moveit_config/launch/planning_pipeline.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/ur3_moveit_config/launch/ros_controllers.launch b/ur3_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..c412631f6 --- /dev/null +++ b/ur3_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur3_moveit_config/launch/setup_assistant.launch b/ur3_moveit_config/launch/setup_assistant.launch index 1998a21be..ea44f51c1 100644 --- a/ur3_moveit_config/launch/setup_assistant.launch +++ b/ur3_moveit_config/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -7,9 +7,9 @@ - diff --git a/ur3_moveit_config/launch/ur3_moveit_controller_manager.launch.xml b/ur3_moveit_config/launch/ur3_moveit_controller_manager.launch.xml index 380c63075..e1dd977da 100644 --- a/ur3_moveit_config/launch/ur3_moveit_controller_manager.launch.xml +++ b/ur3_moveit_config/launch/ur3_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch b/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch index 81b7ca8a7..2631567cc 100644 --- a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch +++ b/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - + - diff --git a/ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml b/ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..eb0bd44b8 --- /dev/null +++ b/ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml b/ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur3_moveit_config/launch/warehouse.launch b/ur3_moveit_config/launch/warehouse.launch index c42dc3b72..767fbc688 100644 --- a/ur3_moveit_config/launch/warehouse.launch +++ b/ur3_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - + - + diff --git a/ur3_moveit_config/launch/warehouse_settings.launch.xml b/ur3_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur3_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur3_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur3_moveit_config/package.xml b/ur3_moveit_config/package.xml index 3a0920c22..67301f293 100644 --- a/ur3_moveit_config/package.xml +++ b/ur3_moveit_config/package.xml @@ -24,6 +24,7 @@ moveit_simple_controller_manager joint_state_publisher robot_state_publisher + tf2_ros xacro ur_description diff --git a/ur3_moveit_config/tests/moveit_planning_execution.xml b/ur3_moveit_config/tests/moveit_planning_execution.xml index ca37b19ed..3c6c18d7f 100644 --- a/ur3_moveit_config/tests/moveit_planning_execution.xml +++ b/ur3_moveit_config/tests/moveit_planning_execution.xml @@ -1,16 +1,14 @@ - - - + - - + + From 67add089e9c75bbba0a431dc54bd7b08eaea7ccc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 13:46:05 +0100 Subject: [PATCH 05/28] Updated ur3e moveit config --- universal_robots/package.xml | 2 +- ur3_e_moveit_config/.setup_assistant | 8 - ur3_e_moveit_config/CHANGELOG.rst | 10 - ur3_e_moveit_config/config/controllers.yaml | 11 - ur3_e_moveit_config/config/ompl_planning.yaml | 84 --- .../launch/default_warehouse_db.launch | 13 - ur3_e_moveit_config/launch/demo.launch | 46 -- .../launch/joystick_control.launch | 17 - ur3_e_moveit_config/launch/moveit.rviz | 290 -------- ur3_e_moveit_config/launch/moveit_rviz.launch | 16 - .../tests/moveit_planning_execution.xml | 18 - ur3e_moveit_config/.setup_assistant | 11 + ur3e_moveit_config/CHANGELOG.rst | 63 ++ .../CMakeLists.txt | 2 +- ur3e_moveit_config/config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 5 +- .../config/joint_limits.yaml | 40 +- .../config/kinematics.yaml | 5 + ur3e_moveit_config/config/ompl_planning.yaml | 172 +++++ .../config/ros_controllers.yaml | 34 + .../config/ur3e.srdf | 16 +- .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 15 + ur3e_moveit_config/launch/demo.launch | 63 ++ .../fake_moveit_controller_manager.launch.xml | 2 +- .../launch/move_group.launch | 58 +- ur3e_moveit_config/launch/moveit.rviz | 689 ++++++++++++++++++ ur3e_moveit_config/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 4 +- .../launch/planning_context.launch | 17 +- .../launch/planning_pipeline.launch.xml | 6 +- .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 8 +- .../launch/sensor_manager.launch.xml | 4 +- .../launch/setup_assistant.launch | 8 +- .../launch/trajectory_execution.launch.xml | 10 +- .../ur3e_moveit_controller_manager.launch.xml | 2 +- .../ur3e_moveit_planning_execution.launch | 8 +- .../ur3e_moveit_sensor_manager.launch.xml | 0 ...robot_moveit_controller_manager.launch.xml | 10 + ...r3e_robot_moveit_sensor_manager.launch.xml | 3 + .../launch/warehouse.launch | 8 +- .../launch/warehouse_settings.launch.xml | 9 +- .../package.xml | 7 +- .../tests/moveit_planning_execution.xml | 16 + 45 files changed, 1257 insertions(+), 617 deletions(-) delete mode 100644 ur3_e_moveit_config/.setup_assistant delete mode 100644 ur3_e_moveit_config/CHANGELOG.rst delete mode 100644 ur3_e_moveit_config/config/controllers.yaml delete mode 100644 ur3_e_moveit_config/config/ompl_planning.yaml delete mode 100644 ur3_e_moveit_config/launch/default_warehouse_db.launch delete mode 100644 ur3_e_moveit_config/launch/demo.launch delete mode 100644 ur3_e_moveit_config/launch/joystick_control.launch delete mode 100644 ur3_e_moveit_config/launch/moveit.rviz delete mode 100644 ur3_e_moveit_config/launch/moveit_rviz.launch delete mode 100644 ur3_e_moveit_config/tests/moveit_planning_execution.xml create mode 100644 ur3e_moveit_config/.setup_assistant create mode 100644 ur3e_moveit_config/CHANGELOG.rst rename {ur3_e_moveit_config => ur3e_moveit_config}/CMakeLists.txt (92%) create mode 100644 ur3e_moveit_config/config/chomp_planning.yaml rename {ur3_e_moveit_config => ur3e_moveit_config}/config/fake_controllers.yaml (75%) rename {ur3_e_moveit_config => ur3e_moveit_config}/config/joint_limits.yaml (51%) rename {ur3_e_moveit_config => ur3e_moveit_config}/config/kinematics.yaml (50%) create mode 100644 ur3e_moveit_config/config/ompl_planning.yaml create mode 100644 ur3e_moveit_config/config/ros_controllers.yaml rename {ur3_e_moveit_config => ur3e_moveit_config}/config/ur3e.srdf (84%) create mode 100644 ur3e_moveit_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 ur3e_moveit_config/launch/default_warehouse_db.launch create mode 100644 ur3e_moveit_config/launch/demo.launch rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/fake_moveit_controller_manager.launch.xml (78%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/move_group.launch (56%) create mode 100644 ur3e_moveit_config/launch/moveit.rviz create mode 100644 ur3e_moveit_config/launch/moveit_rviz.launch rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/ompl_planning_pipeline.launch.xml (90%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/planning_context.launch (55%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/planning_pipeline.launch.xml (57%) create mode 100644 ur3e_moveit_config/launch/ros_controllers.launch rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/run_benchmark_ompl.launch (63%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/sensor_manager.launch.xml (73%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/setup_assistant.launch (60%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/trajectory_execution.launch.xml (80%) rename ur3_e_moveit_config/launch/ur3_e_moveit_controller_manager.launch.xml => ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml (78%) rename ur3_e_moveit_config/launch/ur3_e_moveit_planning_execution.launch => ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch (65%) rename ur3_e_moveit_config/launch/ur3_e_moveit_sensor_manager.launch.xml => ur3e_moveit_config/launch/ur3e_moveit_sensor_manager.launch.xml (100%) create mode 100644 ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml create mode 100644 ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/warehouse.launch (67%) rename {ur3_e_moveit_config => ur3e_moveit_config}/launch/warehouse_settings.launch.xml (64%) rename {ur3_e_moveit_config => ur3e_moveit_config}/package.xml (94%) create mode 100644 ur3e_moveit_config/tests/moveit_planning_execution.xml diff --git a/universal_robots/package.xml b/universal_robots/package.xml index 2a4a0b082..26caecd0c 100644 --- a/universal_robots/package.xml +++ b/universal_robots/package.xml @@ -22,7 +22,7 @@ ur10e_moveit_config ur10_moveit_config - ur3_e_moveit_config + ur3e_moveit_config ur3_moveit_config ur5_e_moveit_config ur5_moveit_config diff --git a/ur3_e_moveit_config/.setup_assistant b/ur3_e_moveit_config/.setup_assistant deleted file mode 100644 index 5d5242684..000000000 --- a/ur3_e_moveit_config/.setup_assistant +++ /dev/null @@ -1,8 +0,0 @@ -moveit_setup_assistant_config: - URDF: - package: ur_e_description - relative_path: urdf/ur3_e_robot.urdf.xacro - SRDF: - relative_path: config/ur3e.srdf - CONFIG: - generated_timestamp: 1438427891 diff --git a/ur3_e_moveit_config/CHANGELOG.rst b/ur3_e_moveit_config/CHANGELOG.rst deleted file mode 100644 index 6c0d15efe..000000000 --- a/ur3_e_moveit_config/CHANGELOG.rst +++ /dev/null @@ -1,10 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ur3_e_moveit_config -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.5 (2019-04-05) ------------------- -* First release (of this package) -* Update maintainer listing: add Miguel (`#410 `_) -* UR-E Series (`#380 `_) -* Contributors: Dave Niewinski, gavanderhoorn diff --git a/ur3_e_moveit_config/config/controllers.yaml b/ur3_e_moveit_config/config/controllers.yaml deleted file mode 100644 index bf2252cb2..000000000 --- a/ur3_e_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur3_e_moveit_config/config/ompl_planning.yaml b/ur3_e_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index 191879b4e..000000000 --- a/ur3_e_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,84 +0,0 @@ -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 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 -manipulator: - default_planner_config: RRTConnectkConfigDefault - 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 -endeffector: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault diff --git a/ur3_e_moveit_config/launch/default_warehouse_db.launch b/ur3_e_moveit_config/launch/default_warehouse_db.launch deleted file mode 100644 index b4b5a0399..000000000 --- a/ur3_e_moveit_config/launch/default_warehouse_db.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/ur3_e_moveit_config/launch/demo.launch b/ur3_e_moveit_config/launch/demo.launch deleted file mode 100644 index 13086b9f7..000000000 --- a/ur3_e_moveit_config/launch/demo.launch +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ur3_e_moveit_config/launch/joystick_control.launch b/ur3_e_moveit_config/launch/joystick_control.launch deleted file mode 100644 index f74173527..000000000 --- a/ur3_e_moveit_config/launch/joystick_control.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/ur3_e_moveit_config/launch/moveit.rviz b/ur3_e_moveit_config/launch/moveit.rviz deleted file mode 100644 index c7540069f..000000000 --- a/ur3_e_moveit_config/launch/moveit.rviz +++ /dev/null @@ -1,290 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 75 - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 299 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ee_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.08 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15 - Joint Violation Color: 255; 0; 255 - Planning Group: manipulator - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.9 - Scene Color: 50; 230; 50 - Scene Display Time: 0.2 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ee_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.82854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 diff --git a/ur3_e_moveit_config/launch/moveit_rviz.launch b/ur3_e_moveit_config/launch/moveit_rviz.launch deleted file mode 100644 index af66a0fee..000000000 --- a/ur3_e_moveit_config/launch/moveit_rviz.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/ur3_e_moveit_config/tests/moveit_planning_execution.xml b/ur3_e_moveit_config/tests/moveit_planning_execution.xml deleted file mode 100644 index 266c83231..000000000 --- a/ur3_e_moveit_config/tests/moveit_planning_execution.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/ur3e_moveit_config/.setup_assistant b/ur3e_moveit_config/.setup_assistant new file mode 100644 index 000000000..ee6c9797c --- /dev/null +++ b/ur3e_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: ur_description + relative_path: urdf/ur3e.xacro + xacro_args: "" + SRDF: + relative_path: config/ur3e.srdf + CONFIG: + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur3e_moveit_config/CHANGELOG.rst b/ur3e_moveit_config/CHANGELOG.rst new file mode 100644 index 000000000..c73e554de --- /dev/null +++ b/ur3e_moveit_config/CHANGELOG.rst @@ -0,0 +1,63 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ur3e_moveit_config +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.5 (2019-04-05) +------------------ +* Update maintainer listing: add Miguel (`#410 `_) +* MoveGroupExecuteService is Deprecated by MoveIt! (`#391 `_) +* Update maintainer and author information. +* Add roslaunch tests (`#362 `_) +* Contributors: gavanderhoorn, Nadia Hammoudeh García, 薯片半价 + +1.2.1 (2018-01-06) +------------------ +* Reduce longest valid segment fraction to accomodate non-limited version of the UR5 (`#266 `_) +* Contributors: Scott Paulin + +1.2.0 (2017-08-04) +------------------ +* Fix Deprecated warning in MoveIt: parameter moved into namespace 'trajectory_execution' +* Contributors: Dave Coleman + +1.1.9 (2017-01-02) +------------------ +* use '--inorder' for jade+ xacro as well. +* make RViz load MoveIt display by default. +* Contributors: gavanderhoorn + +1.1.8 (2016-12-30) +------------------ +* all: update maintainers. +* Contributors: gavanderhoorn + +1.1.7 (2016-12-29) +------------------ +* Don't depend on moveit_plugins metapackage +* Fix xacro warnings in Jade +* Contributors: Dave Coleman, Jon Binney + +1.1.6 (2016-04-01) +------------------ +* add missing dependency for moveit_simple_controller_manager +* Merge branch 'indigo-devel' of github.com:ros-industrial/universal_robot into ur3_moveit_config +* apply latest setup assistant changes to ur5 and ur3e +* Adding comment explaining the choice of default planning algorithm +* Use RRTConnect by default for UR10 + Fixes bug `#193 `_ about slow planning on Indigo + LBKPIECE1 (the previous default) looks to be the wrong planning algorithm for the robot + See https://groups.google.com/forum/#!topic/moveit-users/M71T-GaUNgg +* crop ik solutions wrt joint_limits +* set planning time to 0 +* reduce planning attempts in moveit rviz plugin +* Contributors: Marco Esposito, ipa-fxm + +1.0.2 (2014-03-31) +------------------ + +1.0.1 (2014-03-31) +------------------ +* changes due to file renaming +* update moveit_configs: include ee_link and handle limited robot +* new moveit_configs for ur5 and ur3e +* Contributors: ipa-fxm diff --git a/ur3_e_moveit_config/CMakeLists.txt b/ur3e_moveit_config/CMakeLists.txt similarity index 92% rename from ur3_e_moveit_config/CMakeLists.txt rename to ur3e_moveit_config/CMakeLists.txt index 13e9deb9d..73325ab56 100644 --- a/ur3_e_moveit_config/CMakeLists.txt +++ b/ur3e_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ur3_e_moveit_config) +project(ur3e_moveit_config) find_package(catkin REQUIRED) diff --git a/ur3e_moveit_config/config/chomp_planning.yaml b/ur3e_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur3e_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur3_e_moveit_config/config/fake_controllers.yaml b/ur3e_moveit_config/config/fake_controllers.yaml similarity index 75% rename from ur3_e_moveit_config/config/fake_controllers.yaml rename to ur3e_moveit_config/config/fake_controllers.yaml index 5e24f9451..e328e94d4 100644 --- a/ur3_e_moveit_config/config/fake_controllers.yaml +++ b/ur3e_moveit_config/config/fake_controllers.yaml @@ -9,4 +9,7 @@ controller_list: - wrist_3_joint - name: fake_endeffector_controller joints: - [] \ No newline at end of file + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur3_e_moveit_config/config/joint_limits.yaml b/ur3e_moveit_config/config/joint_limits.yaml similarity index 51% rename from ur3_e_moveit_config/config/joint_limits.yaml rename to ur3e_moveit_config/config/joint_limits.yaml index 991719a66..633b7bbce 100644 --- a/ur3_e_moveit_config/config/joint_limits.yaml +++ b/ur3e_moveit_config/config/joint_limits.yaml @@ -2,33 +2,33 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - shoulder_pan_joint: + elbow_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 - elbow_joint: + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur3_e_moveit_config/config/kinematics.yaml b/ur3e_moveit_config/config/kinematics.yaml similarity index 50% rename from ur3_e_moveit_config/config/kinematics.yaml rename to ur3e_moveit_config/config/kinematics.yaml index 5d492ac1f..e37d336c3 100644 --- a/ur3_e_moveit_config/config/kinematics.yaml +++ b/ur3e_moveit_config/config/kinematics.yaml @@ -1,3 +1,8 @@ +#manipulator: +# kinematics_solver: ur_kinematics/UR10KinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 diff --git a/ur3e_moveit_config/config/ompl_planning.yaml b/ur3e_moveit_config/config/ompl_planning.yaml new file mode 100644 index 000000000..acbf6bd9b --- /dev/null +++ b/ur3e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,172 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + 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 + LBKPIECE: + 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 + BKPIECE: + 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 + KPIECE: + 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 + RRT: + 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 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + 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 + TRRT: + 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 expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 +manipulator: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +endeffector: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur3e_moveit_config/config/ros_controllers.yaml b/ur3e_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..0ecf90959 --- /dev/null +++ b/ur3e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "scaled_pos_joint_traj_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur3_e_moveit_config/config/ur3e.srdf b/ur3e_moveit_config/config/ur3e.srdf similarity index 84% rename from ur3_e_moveit_config/config/ur3e.srdf rename to ur3e_moveit_config/config/ur3e.srdf index b47172108..feb8f539a 100644 --- a/ur3_e_moveit_config/config/ur3e.srdf +++ b/ur3e_moveit_config/config/ur3e.srdf @@ -3,17 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - + - + @@ -33,17 +33,15 @@ - + - - - - + + + - diff --git a/ur3e_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur3e_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..58f736d7c --- /dev/null +++ b/ur3e_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur3e_moveit_config/launch/default_warehouse_db.launch b/ur3e_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 000000000..e938f3d98 --- /dev/null +++ b/ur3e_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/ur3e_moveit_config/launch/demo.launch b/ur3e_moveit_config/launch/demo.launch new file mode 100644 index 000000000..83a323361 --- /dev/null +++ b/ur3e_moveit_config/launch/demo.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur3_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur3e_moveit_config/launch/fake_moveit_controller_manager.launch.xml similarity index 78% rename from ur3_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml rename to ur3e_moveit_config/launch/fake_moveit_controller_manager.launch.xml index 630203a59..8789fd70c 100644 --- a/ur3_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ b/ur3e_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -4,6 +4,6 @@ - + diff --git a/ur3_e_moveit_config/launch/move_group.launch b/ur3e_moveit_config/launch/move_group.launch similarity index 56% rename from ur3_e_moveit_config/launch/move_group.launch rename to ur3e_moveit_config/launch/move_group.launch index de6d39dd3..5c2ab7ade 100644 --- a/ur3_e_moveit_config/launch/move_group.launch +++ b/ur3e_moveit_config/launch/move_group.launch @@ -1,15 +1,10 @@ - - - - - + value="gdb -x $(find ur3e_moveit_config)/launch/gdb_settings.gdb --ex run --args" /> @@ -17,27 +12,52 @@ + + + + + + + + + + + + + + + - - + + - + - + - - + + @@ -48,19 +68,9 @@ + + - - diff --git a/ur3e_moveit_config/launch/moveit.rviz b/ur3e_moveit_config/launch/moveit.rviz new file mode 100644 index 000000000..b431b2a03 --- /dev/null +++ b/ur3e_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur3e_moveit_config/launch/moveit_rviz.launch b/ur3e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..a4605c037 --- /dev/null +++ b/ur3e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur3_e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur3e_moveit_config/launch/ompl_planning_pipeline.launch.xml similarity index 90% rename from ur3_e_moveit_config/launch/ompl_planning_pipeline.launch.xml rename to ur3e_moveit_config/launch/ompl_planning_pipeline.launch.xml index 71475878e..bf3b34698 100644 --- a/ur3_e_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur3e_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - + diff --git a/ur3_e_moveit_config/launch/planning_context.launch b/ur3e_moveit_config/launch/planning_context.launch similarity index 55% rename from ur3_e_moveit_config/launch/planning_context.launch rename to ur3e_moveit_config/launch/planning_context.launch index d1a1618ac..49ef7dfeb 100644 --- a/ur3_e_moveit_config/launch/planning_context.launch +++ b/ur3e_moveit_config/launch/planning_context.launch @@ -1,28 +1,25 @@ - - - - - + - - + + - + - + + - + diff --git a/ur3_e_moveit_config/launch/planning_pipeline.launch.xml b/ur3e_moveit_config/launch/planning_pipeline.launch.xml similarity index 57% rename from ur3_e_moveit_config/launch/planning_pipeline.launch.xml rename to ur3e_moveit_config/launch/planning_pipeline.launch.xml index 40f9e8d96..5dfab7d5a 100644 --- a/ur3_e_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur3e_moveit_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/ur3e_moveit_config/launch/ros_controllers.launch b/ur3e_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..b3e545009 --- /dev/null +++ b/ur3e_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur3_e_moveit_config/launch/run_benchmark_ompl.launch b/ur3e_moveit_config/launch/run_benchmark_ompl.launch similarity index 63% rename from ur3_e_moveit_config/launch/run_benchmark_ompl.launch rename to ur3e_moveit_config/launch/run_benchmark_ompl.launch index e84b5312f..46f8cb9f3 100644 --- a/ur3_e_moveit_config/launch/run_benchmark_ompl.launch +++ b/ur3e_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/ur3_e_moveit_config/launch/sensor_manager.launch.xml b/ur3e_moveit_config/launch/sensor_manager.launch.xml similarity index 73% rename from ur3_e_moveit_config/launch/sensor_manager.launch.xml rename to ur3e_moveit_config/launch/sensor_manager.launch.xml index 8f17c237b..c6a1afaae 100644 --- a/ur3_e_moveit_config/launch/sensor_manager.launch.xml +++ b/ur3e_moveit_config/launch/sensor_manager.launch.xml @@ -8,7 +8,7 @@ - - + + diff --git a/ur3_e_moveit_config/launch/setup_assistant.launch b/ur3e_moveit_config/launch/setup_assistant.launch similarity index 60% rename from ur3_e_moveit_config/launch/setup_assistant.launch rename to ur3e_moveit_config/launch/setup_assistant.launch index ca7e28b0f..215c93dce 100644 --- a/ur3_e_moveit_config/launch/setup_assistant.launch +++ b/ur3e_moveit_config/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -7,9 +7,9 @@ - diff --git a/ur3_e_moveit_config/launch/trajectory_execution.launch.xml b/ur3e_moveit_config/launch/trajectory_execution.launch.xml similarity index 80% rename from ur3_e_moveit_config/launch/trajectory_execution.launch.xml rename to ur3e_moveit_config/launch/trajectory_execution.launch.xml index 6786cca12..aadaeb14c 100644 --- a/ur3_e_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur3e_moveit_config/launch/trajectory_execution.launch.xml @@ -1,6 +1,6 @@ - + @@ -10,9 +10,9 @@ - + - - - + + + diff --git a/ur3_e_moveit_config/launch/ur3_e_moveit_controller_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml similarity index 78% rename from ur3_e_moveit_config/launch/ur3_e_moveit_controller_manager.launch.xml rename to ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml index f54839f4e..ec00c74b6 100644 --- a/ur3_e_moveit_config/launch/ur3_e_moveit_controller_manager.launch.xml +++ b/ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur3_e_moveit_config/launch/ur3_e_moveit_planning_execution.launch b/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch similarity index 65% rename from ur3_e_moveit_config/launch/ur3_e_moveit_planning_execution.launch rename to ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch index 75b83fee8..91de984ad 100644 --- a/ur3_e_moveit_config/launch/ur3_e_moveit_planning_execution.launch +++ b/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - + - - + diff --git a/ur3_e_moveit_config/launch/ur3_e_moveit_sensor_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_moveit_sensor_manager.launch.xml similarity index 100% rename from ur3_e_moveit_config/launch/ur3_e_moveit_sensor_manager.launch.xml rename to ur3e_moveit_config/launch/ur3e_moveit_sensor_manager.launch.xml diff --git a/ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..82dda5ce2 --- /dev/null +++ b/ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur3_e_moveit_config/launch/warehouse.launch b/ur3e_moveit_config/launch/warehouse.launch similarity index 67% rename from ur3_e_moveit_config/launch/warehouse.launch rename to ur3e_moveit_config/launch/warehouse.launch index 918a58717..8916787cb 100644 --- a/ur3_e_moveit_config/launch/warehouse.launch +++ b/ur3e_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - - + + - + diff --git a/ur3_e_moveit_config/launch/warehouse_settings.launch.xml b/ur3e_moveit_config/launch/warehouse_settings.launch.xml similarity index 64% rename from ur3_e_moveit_config/launch/warehouse_settings.launch.xml rename to ur3e_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur3_e_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur3e_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur3_e_moveit_config/package.xml b/ur3e_moveit_config/package.xml similarity index 94% rename from ur3_e_moveit_config/package.xml rename to ur3e_moveit_config/package.xml index bfdb3f9bc..4563bfde3 100644 --- a/ur3_e_moveit_config/package.xml +++ b/ur3e_moveit_config/package.xml @@ -1,7 +1,7 @@ - ur3_e_moveit_config + ur3e_moveit_config 1.2.5 An automatically generated package with all the configuration and launch files for using the ur3e with the MoveIt Motion Planning Framework @@ -10,7 +10,7 @@ G.A. vd. Hoorn Miguel Prada Sarasola Nadia Hammoudeh Garcia - + BSD http://moveit.ros.org/ @@ -24,10 +24,11 @@ moveit_simple_controller_manager joint_state_publisher robot_state_publisher + tf2_ros xacro ur_description roslaunch catkin - + diff --git a/ur3e_moveit_config/tests/moveit_planning_execution.xml b/ur3e_moveit_config/tests/moveit_planning_execution.xml new file mode 100644 index 000000000..81c4302af --- /dev/null +++ b/ur3e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + From 16f5dc661bc41a361e40bdb68f52098cb200408e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 13:47:46 +0100 Subject: [PATCH 06/28] Updated ur5 moveit config --- ur5_moveit_config/.setup_assistant | 7 +- ur5_moveit_config/config/chomp_planning.yaml | 18 + ur5_moveit_config/config/controllers.yaml | 11 - .../config/fake_controllers.yaml | 5 +- ur5_moveit_config/config/joint_limits.yaml | 40 +- ur5_moveit_config/config/kinematics.yaml | 2 +- ur5_moveit_config/config/ompl_planning.yaml | 165 +++-- ur5_moveit_config/config/ros_controllers.yaml | 34 + ur5_moveit_config/config/ur5.srdf | 17 +- .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 6 +- ur5_moveit_config/launch/demo.launch | 51 +- ur5_moveit_config/launch/move_group.launch | 51 +- ur5_moveit_config/launch/moveit.rviz | 615 +++++++++++++++--- ur5_moveit_config/launch/moveit_rviz.launch | 11 +- .../launch/ompl_planning_pipeline.launch.xml | 2 +- .../launch/planning_context.launch | 11 +- .../launch/planning_pipeline.launch.xml | 4 +- .../launch/ros_controllers.launch | 11 + .../launch/setup_assistant.launch | 8 +- .../launch/trajectory_execution.launch.xml | 6 +- .../ur5_moveit_controller_manager.launch.xml | 2 +- .../ur5_moveit_planning_execution.launch | 6 +- ...robot_moveit_controller_manager.launch.xml | 10 + ...ur5_robot_moveit_sensor_manager.launch.xml | 3 + ur5_moveit_config/launch/warehouse.launch | 6 +- .../launch/warehouse_settings.launch.xml | 9 +- ur5_moveit_config/package.xml | 1 + .../tests/moveit_planning_execution.xml | 8 +- 29 files changed, 872 insertions(+), 268 deletions(-) create mode 100644 ur5_moveit_config/config/chomp_planning.yaml delete mode 100644 ur5_moveit_config/config/controllers.yaml create mode 100644 ur5_moveit_config/config/ros_controllers.yaml create mode 100644 ur5_moveit_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 ur5_moveit_config/launch/ros_controllers.launch create mode 100644 ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml create mode 100644 ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml diff --git a/ur5_moveit_config/.setup_assistant b/ur5_moveit_config/.setup_assistant index fccc0b629..12a72528d 100644 --- a/ur5_moveit_config/.setup_assistant +++ b/ur5_moveit_config/.setup_assistant @@ -1,8 +1,11 @@ moveit_setup_assistant_config: URDF: package: ur_description - relative_path: urdf/ur5_robot.urdf.xacro + relative_path: urdf/ur5.xacro + xacro_args: "" SRDF: relative_path: config/ur5.srdf CONFIG: - generated_timestamp: 1413877529 \ No newline at end of file + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur5_moveit_config/config/chomp_planning.yaml b/ur5_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur5_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur5_moveit_config/config/controllers.yaml b/ur5_moveit_config/config/controllers.yaml deleted file mode 100644 index bf2252cb2..000000000 --- a/ur5_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur5_moveit_config/config/fake_controllers.yaml b/ur5_moveit_config/config/fake_controllers.yaml index 5e24f9451..e328e94d4 100644 --- a/ur5_moveit_config/config/fake_controllers.yaml +++ b/ur5_moveit_config/config/fake_controllers.yaml @@ -9,4 +9,7 @@ controller_list: - wrist_3_joint - name: fake_endeffector_controller joints: - [] \ No newline at end of file + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur5_moveit_config/config/joint_limits.yaml b/ur5_moveit_config/config/joint_limits.yaml index 60797d513..633b7bbce 100644 --- a/ur5_moveit_config/config/joint_limits.yaml +++ b/ur5_moveit_config/config/joint_limits.yaml @@ -2,33 +2,33 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - shoulder_pan_joint: + elbow_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 - elbow_joint: + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: true - max_acceleration: 3.15 + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: true - max_acceleration: 3.2 \ No newline at end of file + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml index e2fa9b157..e37d336c3 100644 --- a/ur5_moveit_config/config/kinematics.yaml +++ b/ur5_moveit_config/config/kinematics.yaml @@ -1,5 +1,5 @@ #manipulator: -# kinematics_solver: ur_kinematics/UR5KinematicsPlugin +# kinematics_solver: ur_kinematics/UR10KinematicsPlugin # kinematics_solver_search_resolution: 0.005 # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 diff --git a/ur5_moveit_config/config/ompl_planning.yaml b/ur5_moveit_config/config/ompl_planning.yaml index b58b2d655..acbf6bd9b 100644 --- a/ur5_moveit_config/config/ompl_planning.yaml +++ b/ur5_moveit_config/config/ompl_planning.yaml @@ -1,42 +1,42 @@ planner_configs: - SBLkConfigDefault: + SBL: type: geometric::SBL range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: + EST: 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: + LBKPIECE: 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: + BKPIECE: 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: + KPIECE: 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 + 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: + RRT: 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: + RRTConnect: type: geometric::RRTConnect range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: + RRTstar: 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: + TRRT: 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 @@ -44,40 +44,129 @@ planner_configs: 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() + 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 expresssion. default: 0.0 set in setup() - PRMkConfigDefault: + PRM: type: geometric::PRM max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: + PRMstar: type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 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 + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo endeffector: planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur5_moveit_config/config/ros_controllers.yaml b/ur5_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..0ecf90959 --- /dev/null +++ b/ur5_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "scaled_pos_joint_traj_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur5_moveit_config/config/ur5.srdf b/ur5_moveit_config/config/ur5.srdf index 9b13cebea..ddf9b0f37 100644 --- a/ur5_moveit_config/config/ur5.srdf +++ b/ur5_moveit_config/config/ur5.srdf @@ -3,17 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - + - + @@ -33,17 +33,18 @@ - + - - - - + + + + + diff --git a/ur5_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur5_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..b0f9b159f --- /dev/null +++ b/ur5_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur5_moveit_config/launch/default_warehouse_db.launch b/ur5_moveit_config/launch/default_warehouse_db.launch index 0399e7246..c71e519a1 100644 --- a/ur5_moveit_config/launch/default_warehouse_db.launch +++ b/ur5_moveit_config/launch/default_warehouse_db.launch @@ -1,10 +1,12 @@ + + - + - + diff --git a/ur5_moveit_config/launch/demo.launch b/ur5_moveit_config/launch/demo.launch index c4befef84..0b0a89bcc 100644 --- a/ur5_moveit_config/launch/demo.launch +++ b/ur5_moveit_config/launch/demo.launch @@ -1,46 +1,63 @@ + + + + + - - - - - - - + + + + + + - + - - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] - + - + - + + + - - + + - + + + diff --git a/ur5_moveit_config/launch/move_group.launch b/ur5_moveit_config/launch/move_group.launch index 5e13ea380..fe73c1d3a 100644 --- a/ur5_moveit_config/launch/move_group.launch +++ b/ur5_moveit_config/launch/move_group.launch @@ -1,14 +1,10 @@ - - - - - - + @@ -16,15 +12,40 @@ + + + + + + + + + + + + + + + - + @@ -36,7 +57,7 @@ - + @@ -47,19 +68,9 @@ + + - - diff --git a/ur5_moveit_config/launch/moveit.rviz b/ur5_moveit_config/launch/moveit.rviz index c7540069f..b431b2a03 100644 --- a/ur5_moveit_config/launch/moveit.rviz +++ b/ur5_moveit_config/launch/moveit.rviz @@ -1,31 +1,19 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 84 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 299 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: @@ -49,106 +37,314 @@ Visualization Manager: Value: true - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: Alpha: 1 Show Axes: false Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ee_link: + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - forearm_link: + l_gripper_l_finger_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - shoulder_link: + l_gripper_l_finger_tip_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - tool0: + l_gripper_motor_accelerometer_link: Alpha: 1 Show Axes: false Show Trail: false - upper_arm_link: + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - world: + r_gripper_r_finger_link: Alpha: 1 Show Axes: false Show Trail: false - wrist_1_link: + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_2_link: + r_shoulder_pan_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_3_link: + r_upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - Loop Animation: false + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.08 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15 + Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: manipulator + Planning Group: left_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -157,7 +353,7 @@ Visualization Manager: Planning Scene Topic: move_group/monitored_planning_scene Robot Description: robot_description Scene Geometry: - Scene Alpha: 0.9 + Scene Alpha: 1 Scene Color: 50; 230; 50 Scene Display Time: 0.2 Show Scene Geometry: true @@ -166,125 +362,328 @@ Visualization Manager: Scene Robot: Attached Body Color: 150; 50; 150 Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: Alpha: 1 Show Axes: false Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - ee_link: + bl_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - forearm_link: + bl_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - shoulder_link: + bl_caster_rotation_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - tool0: + br_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false - upper_arm_link: + Value: true + br_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - world: + br_caster_rotation_link: Alpha: 1 Show Axes: false Show Trail: false - wrist_1_link: + Value: true + double_stereo_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_2_link: + fl_caster_l_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wrist_3_link: + fl_caster_r_wheel_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 + Fixed Frame: base_link Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.82854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false + Class: rviz/XYOrbit + Distance: 2.9965 Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 + Height: 1337 + Help: + collapsed: false Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Motion Planning: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur5_moveit_config/launch/moveit_rviz.launch b/ur5_moveit_config/launch/moveit_rviz.launch index c8216c755..a4605c037 100644 --- a/ur5_moveit_config/launch/moveit_rviz.launch +++ b/ur5_moveit_config/launch/moveit_rviz.launch @@ -4,13 +4,12 @@ - - - - + + + + - + args="$(arg command_args)" output="screen"> diff --git a/ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml index 43eb59b57..89c4bb85d 100644 --- a/ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - - - - - + - + @@ -23,6 +19,7 @@ + - + diff --git a/ur5_moveit_config/launch/planning_pipeline.launch.xml b/ur5_moveit_config/launch/planning_pipeline.launch.xml index 3d1cb9297..89fcea057 100644 --- a/ur5_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur5_moveit_config/launch/planning_pipeline.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/ur5_moveit_config/launch/ros_controllers.launch b/ur5_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..fb90fcc68 --- /dev/null +++ b/ur5_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur5_moveit_config/launch/setup_assistant.launch b/ur5_moveit_config/launch/setup_assistant.launch index 4ca5f964e..9f42d38ae 100644 --- a/ur5_moveit_config/launch/setup_assistant.launch +++ b/ur5_moveit_config/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -7,9 +7,9 @@ - diff --git a/ur5_moveit_config/launch/trajectory_execution.launch.xml b/ur5_moveit_config/launch/trajectory_execution.launch.xml index 472bc6e1b..8b6beec1f 100644 --- a/ur5_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur5_moveit_config/launch/trajectory_execution.launch.xml @@ -1,6 +1,6 @@ - + @@ -10,9 +10,9 @@ - + - + diff --git a/ur5_moveit_config/launch/ur5_moveit_controller_manager.launch.xml b/ur5_moveit_config/launch/ur5_moveit_controller_manager.launch.xml index db01518be..2ca68c92a 100644 --- a/ur5_moveit_config/launch/ur5_moveit_controller_manager.launch.xml +++ b/ur5_moveit_config/launch/ur5_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch b/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch index d2d697711..e1c225159 100644 --- a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch +++ b/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - + - diff --git a/ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml b/ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..0500324ce --- /dev/null +++ b/ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml b/ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur5_moveit_config/launch/warehouse.launch b/ur5_moveit_config/launch/warehouse.launch index 579786c40..99b0b6aff 100644 --- a/ur5_moveit_config/launch/warehouse.launch +++ b/ur5_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - + - + diff --git a/ur5_moveit_config/launch/warehouse_settings.launch.xml b/ur5_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur5_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur5_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur5_moveit_config/package.xml b/ur5_moveit_config/package.xml index cd6507462..a2d2e00a9 100644 --- a/ur5_moveit_config/package.xml +++ b/ur5_moveit_config/package.xml @@ -24,6 +24,7 @@ moveit_simple_controller_manager joint_state_publisher robot_state_publisher + tf2_ros xacro ur_description diff --git a/ur5_moveit_config/tests/moveit_planning_execution.xml b/ur5_moveit_config/tests/moveit_planning_execution.xml index d566fa9db..e3f3ae0ad 100644 --- a/ur5_moveit_config/tests/moveit_planning_execution.xml +++ b/ur5_moveit_config/tests/moveit_planning_execution.xml @@ -1,16 +1,14 @@ - - - + - - + + From c234acad8179704682855f3c5ce0cc889bc5fb6d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 13:49:14 +0100 Subject: [PATCH 07/28] Updated ur5e moveit config --- universal_robots/package.xml | 2 +- ur5_e_moveit_config/.setup_assistant | 8 - ur5_e_moveit_config/config/controllers.yaml | 11 - ur5_e_moveit_config/config/ompl_planning.yaml | 84 --- .../launch/default_warehouse_db.launch | 13 - ur5_e_moveit_config/launch/demo.launch | 46 -- ur5_e_moveit_config/launch/moveit.rviz | 290 -------- ur5_e_moveit_config/launch/moveit_rviz.launch | 16 - .../tests/moveit_planning_execution.xml | 18 - ur5e_moveit_config/.setup_assistant | 11 + ur5e_moveit_config/CHANGELOG.rst | 63 ++ .../CMakeLists.txt | 2 +- ur5e_moveit_config/config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 5 +- .../config/joint_limits.yaml | 40 +- .../config/kinematics.yaml | 5 + ur5e_moveit_config/config/ompl_planning.yaml | 172 +++++ .../config/ros_controllers.yaml | 34 + .../config/ur5e.srdf | 17 +- .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 15 + ur5e_moveit_config/launch/demo.launch | 63 ++ .../fake_moveit_controller_manager.launch.xml | 2 +- .../launch/move_group.launch | 59 +- ur5e_moveit_config/launch/moveit.rviz | 689 ++++++++++++++++++ ur5e_moveit_config/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 4 +- .../launch/planning_context.launch | 17 +- .../launch/planning_pipeline.launch.xml | 6 +- .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 8 +- .../launch/sensor_manager.launch.xml | 4 +- .../launch/setup_assistant.launch | 8 +- .../launch/trajectory_execution.launch.xml | 10 +- .../ur5e_moveit_controller_manager.launch.xml | 2 +- .../ur5e_moveit_planning_execution.launch | 8 +- .../ur5e_moveit_sensor_manager.launch.xml | 0 ...robot_moveit_controller_manager.launch.xml | 10 + ...r5e_robot_moveit_sensor_manager.launch.xml | 3 + .../launch/warehouse.launch | 8 +- .../launch/warehouse_settings.launch.xml | 9 +- .../package.xml | 7 +- .../tests/moveit_planning_execution.xml | 16 + 43 files changed, 1260 insertions(+), 589 deletions(-) delete mode 100644 ur5_e_moveit_config/.setup_assistant delete mode 100644 ur5_e_moveit_config/config/controllers.yaml delete mode 100644 ur5_e_moveit_config/config/ompl_planning.yaml delete mode 100644 ur5_e_moveit_config/launch/default_warehouse_db.launch delete mode 100644 ur5_e_moveit_config/launch/demo.launch delete mode 100644 ur5_e_moveit_config/launch/moveit.rviz delete mode 100644 ur5_e_moveit_config/launch/moveit_rviz.launch delete mode 100644 ur5_e_moveit_config/tests/moveit_planning_execution.xml create mode 100644 ur5e_moveit_config/.setup_assistant create mode 100644 ur5e_moveit_config/CHANGELOG.rst rename {ur5_e_moveit_config => ur5e_moveit_config}/CMakeLists.txt (92%) create mode 100644 ur5e_moveit_config/config/chomp_planning.yaml rename {ur5_e_moveit_config => ur5e_moveit_config}/config/fake_controllers.yaml (75%) rename {ur5_e_moveit_config => ur5e_moveit_config}/config/joint_limits.yaml (51%) rename {ur5_e_moveit_config => ur5e_moveit_config}/config/kinematics.yaml (50%) create mode 100644 ur5e_moveit_config/config/ompl_planning.yaml create mode 100644 ur5e_moveit_config/config/ros_controllers.yaml rename {ur5_e_moveit_config => ur5e_moveit_config}/config/ur5e.srdf (83%) create mode 100644 ur5e_moveit_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 ur5e_moveit_config/launch/default_warehouse_db.launch create mode 100644 ur5e_moveit_config/launch/demo.launch rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/fake_moveit_controller_manager.launch.xml (78%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/move_group.launch (54%) create mode 100644 ur5e_moveit_config/launch/moveit.rviz create mode 100644 ur5e_moveit_config/launch/moveit_rviz.launch rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/ompl_planning_pipeline.launch.xml (90%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/planning_context.launch (55%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/planning_pipeline.launch.xml (57%) create mode 100644 ur5e_moveit_config/launch/ros_controllers.launch rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/run_benchmark_ompl.launch (63%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/sensor_manager.launch.xml (73%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/setup_assistant.launch (60%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/trajectory_execution.launch.xml (80%) rename ur5_e_moveit_config/launch/ur5_e_moveit_controller_manager.launch.xml => ur5e_moveit_config/launch/ur5e_moveit_controller_manager.launch.xml (78%) rename ur5_e_moveit_config/launch/ur5_e_moveit_planning_execution.launch => ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch (65%) rename ur5_e_moveit_config/launch/ur5_e_moveit_sensor_manager.launch.xml => ur5e_moveit_config/launch/ur5e_moveit_sensor_manager.launch.xml (100%) create mode 100644 ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml create mode 100644 ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/warehouse.launch (67%) rename {ur5_e_moveit_config => ur5e_moveit_config}/launch/warehouse_settings.launch.xml (64%) rename {ur5_e_moveit_config => ur5e_moveit_config}/package.xml (94%) create mode 100644 ur5e_moveit_config/tests/moveit_planning_execution.xml diff --git a/universal_robots/package.xml b/universal_robots/package.xml index 26caecd0c..0593da5df 100644 --- a/universal_robots/package.xml +++ b/universal_robots/package.xml @@ -24,7 +24,7 @@ ur10_moveit_config ur3e_moveit_config ur3_moveit_config - ur5_e_moveit_config + ur5e_moveit_config ur5_moveit_config ur_description ur_gazebo diff --git a/ur5_e_moveit_config/.setup_assistant b/ur5_e_moveit_config/.setup_assistant deleted file mode 100644 index dd22d1333..000000000 --- a/ur5_e_moveit_config/.setup_assistant +++ /dev/null @@ -1,8 +0,0 @@ -moveit_setup_assistant_config: - URDF: - package: ur_e_description - relative_path: urdf/ur5e_robot.urdf.xacro - SRDF: - relative_path: config/ur5e.srdf - CONFIG: - generated_timestamp: 1413877529 diff --git a/ur5_e_moveit_config/config/controllers.yaml b/ur5_e_moveit_config/config/controllers.yaml deleted file mode 100644 index bf2252cb2..000000000 --- a/ur5_e_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,11 +0,0 @@ -controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint diff --git a/ur5_e_moveit_config/config/ompl_planning.yaml b/ur5_e_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index 191879b4e..000000000 --- a/ur5_e_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,84 +0,0 @@ -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 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 -manipulator: - default_planner_config: RRTConnectkConfigDefault - 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 -endeffector: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault diff --git a/ur5_e_moveit_config/launch/default_warehouse_db.launch b/ur5_e_moveit_config/launch/default_warehouse_db.launch deleted file mode 100644 index 626b22f68..000000000 --- a/ur5_e_moveit_config/launch/default_warehouse_db.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/ur5_e_moveit_config/launch/demo.launch b/ur5_e_moveit_config/launch/demo.launch deleted file mode 100644 index dd23ff7d8..000000000 --- a/ur5_e_moveit_config/launch/demo.launch +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ur5_e_moveit_config/launch/moveit.rviz b/ur5_e_moveit_config/launch/moveit.rviz deleted file mode 100644 index c7540069f..000000000 --- a/ur5_e_moveit_config/launch/moveit.rviz +++ /dev/null @@ -1,290 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 75 - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 299 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ee_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.08 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15 - Joint Violation Color: 255; 0; 255 - Planning Group: manipulator - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.9 - Scene Color: 50; 230; 50 - Scene Display Time: 0.2 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ee_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.82854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 diff --git a/ur5_e_moveit_config/launch/moveit_rviz.launch b/ur5_e_moveit_config/launch/moveit_rviz.launch deleted file mode 100644 index a1cb30986..000000000 --- a/ur5_e_moveit_config/launch/moveit_rviz.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/ur5_e_moveit_config/tests/moveit_planning_execution.xml b/ur5_e_moveit_config/tests/moveit_planning_execution.xml deleted file mode 100644 index 53f6e391f..000000000 --- a/ur5_e_moveit_config/tests/moveit_planning_execution.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/ur5e_moveit_config/.setup_assistant b/ur5e_moveit_config/.setup_assistant new file mode 100644 index 000000000..3fae5acd7 --- /dev/null +++ b/ur5e_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: ur_description + relative_path: urdf/ur5e.xacro + xacro_args: "" + SRDF: + relative_path: config/ur5e.srdf + CONFIG: + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur5e_moveit_config/CHANGELOG.rst b/ur5e_moveit_config/CHANGELOG.rst new file mode 100644 index 000000000..28eb11545 --- /dev/null +++ b/ur5e_moveit_config/CHANGELOG.rst @@ -0,0 +1,63 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ur5e_moveit_config +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.5 (2019-04-05) +------------------ +* Update maintainer listing: add Miguel (`#410 `_) +* MoveGroupExecuteService is Deprecated by MoveIt! (`#391 `_) +* Update maintainer and author information. +* Add roslaunch tests (`#362 `_) +* Contributors: gavanderhoorn, Nadia Hammoudeh García, 薯片半价 + +1.2.1 (2018-01-06) +------------------ +* Reduce longest valid segment fraction to accomodate non-limited version of the UR5 (`#266 `_) +* Contributors: Scott Paulin + +1.2.0 (2017-08-04) +------------------ +* Fix Deprecated warning in MoveIt: parameter moved into namespace 'trajectory_execution' +* Contributors: Dave Coleman + +1.1.9 (2017-01-02) +------------------ +* use '--inorder' for jade+ xacro as well. +* make RViz load MoveIt display by default. +* Contributors: gavanderhoorn + +1.1.8 (2016-12-30) +------------------ +* all: update maintainers. +* Contributors: gavanderhoorn + +1.1.7 (2016-12-29) +------------------ +* Don't depend on moveit_plugins metapackage +* Fix xacro warnings in Jade +* Contributors: Dave Coleman, Jon Binney + +1.1.6 (2016-04-01) +------------------ +* add missing dependency for moveit_simple_controller_manager +* Merge branch 'indigo-devel' of github.com:ros-industrial/universal_robot into ur3_moveit_config +* apply latest setup assistant changes to ur5 and ur5e +* Adding comment explaining the choice of default planning algorithm +* Use RRTConnect by default for UR10 + Fixes bug `#193 `_ about slow planning on Indigo + LBKPIECE1 (the previous default) looks to be the wrong planning algorithm for the robot + See https://groups.google.com/forum/#!topic/moveit-users/M71T-GaUNgg +* crop ik solutions wrt joint_limits +* set planning time to 0 +* reduce planning attempts in moveit rviz plugin +* Contributors: Marco Esposito, ipa-fxm + +1.0.2 (2014-03-31) +------------------ + +1.0.1 (2014-03-31) +------------------ +* changes due to file renaming +* update moveit_configs: include ee_link and handle limited robot +* new moveit_configs for ur5 and ur5e +* Contributors: ipa-fxm diff --git a/ur5_e_moveit_config/CMakeLists.txt b/ur5e_moveit_config/CMakeLists.txt similarity index 92% rename from ur5_e_moveit_config/CMakeLists.txt rename to ur5e_moveit_config/CMakeLists.txt index 8e4d03bd5..0c83d4541 100644 --- a/ur5_e_moveit_config/CMakeLists.txt +++ b/ur5e_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ur5_e_moveit_config) +project(ur5e_moveit_config) find_package(catkin REQUIRED) diff --git a/ur5e_moveit_config/config/chomp_planning.yaml b/ur5e_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur5e_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur5_e_moveit_config/config/fake_controllers.yaml b/ur5e_moveit_config/config/fake_controllers.yaml similarity index 75% rename from ur5_e_moveit_config/config/fake_controllers.yaml rename to ur5e_moveit_config/config/fake_controllers.yaml index 5e24f9451..e328e94d4 100644 --- a/ur5_e_moveit_config/config/fake_controllers.yaml +++ b/ur5e_moveit_config/config/fake_controllers.yaml @@ -9,4 +9,7 @@ controller_list: - wrist_3_joint - name: fake_endeffector_controller joints: - [] \ No newline at end of file + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur5_e_moveit_config/config/joint_limits.yaml b/ur5e_moveit_config/config/joint_limits.yaml similarity index 51% rename from ur5_e_moveit_config/config/joint_limits.yaml rename to ur5e_moveit_config/config/joint_limits.yaml index 991719a66..633b7bbce 100644 --- a/ur5_e_moveit_config/config/joint_limits.yaml +++ b/ur5e_moveit_config/config/joint_limits.yaml @@ -2,33 +2,33 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - shoulder_pan_joint: + elbow_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 - elbow_joint: + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: has_velocity_limits: true - max_velocity: 3.14 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 6.28 - has_acceleration_limits: true - max_acceleration: 3.14 + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur5_e_moveit_config/config/kinematics.yaml b/ur5e_moveit_config/config/kinematics.yaml similarity index 50% rename from ur5_e_moveit_config/config/kinematics.yaml rename to ur5e_moveit_config/config/kinematics.yaml index 5d492ac1f..e37d336c3 100644 --- a/ur5_e_moveit_config/config/kinematics.yaml +++ b/ur5e_moveit_config/config/kinematics.yaml @@ -1,3 +1,8 @@ +#manipulator: +# kinematics_solver: ur_kinematics/UR10KinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 diff --git a/ur5e_moveit_config/config/ompl_planning.yaml b/ur5e_moveit_config/config/ompl_planning.yaml new file mode 100644 index 000000000..acbf6bd9b --- /dev/null +++ b/ur5e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,172 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + 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 + LBKPIECE: + 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 + BKPIECE: + 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 + KPIECE: + 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 + RRT: + 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 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + 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 + TRRT: + 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 expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 +manipulator: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +endeffector: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur5e_moveit_config/config/ros_controllers.yaml b/ur5e_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..0ecf90959 --- /dev/null +++ b/ur5e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "scaled_pos_joint_traj_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur5_e_moveit_config/config/ur5e.srdf b/ur5e_moveit_config/config/ur5e.srdf similarity index 83% rename from ur5_e_moveit_config/config/ur5e.srdf rename to ur5e_moveit_config/config/ur5e.srdf index bb009a906..76bc9d0b8 100644 --- a/ur5_e_moveit_config/config/ur5e.srdf +++ b/ur5e_moveit_config/config/ur5e.srdf @@ -3,17 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + - + - + @@ -33,17 +33,18 @@ - + - - - - + + + + + diff --git a/ur5e_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur5e_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..ed0bc619d --- /dev/null +++ b/ur5e_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/default_warehouse_db.launch b/ur5e_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 000000000..c4baf5d47 --- /dev/null +++ b/ur5e_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/demo.launch b/ur5e_moveit_config/launch/demo.launch new file mode 100644 index 000000000..d00d93522 --- /dev/null +++ b/ur5e_moveit_config/launch/demo.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur5_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur5e_moveit_config/launch/fake_moveit_controller_manager.launch.xml similarity index 78% rename from ur5_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml rename to ur5e_moveit_config/launch/fake_moveit_controller_manager.launch.xml index 8a8e020d7..0e7d73907 100644 --- a/ur5_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ b/ur5e_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -4,6 +4,6 @@ - + diff --git a/ur5_e_moveit_config/launch/move_group.launch b/ur5e_moveit_config/launch/move_group.launch similarity index 54% rename from ur5_e_moveit_config/launch/move_group.launch rename to ur5e_moveit_config/launch/move_group.launch index d39c94716..764c400bf 100644 --- a/ur5_e_moveit_config/launch/move_group.launch +++ b/ur5e_moveit_config/launch/move_group.launch @@ -1,14 +1,10 @@ - - - - - - + @@ -16,27 +12,52 @@ + + + + + + + + + + + + + + + - - + + - + - + - - + + @@ -47,19 +68,9 @@ + + - - diff --git a/ur5e_moveit_config/launch/moveit.rviz b/ur5e_moveit_config/launch/moveit.rviz new file mode 100644 index 000000000..b431b2a03 --- /dev/null +++ b/ur5e_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur5e_moveit_config/launch/moveit_rviz.launch b/ur5e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..a4605c037 --- /dev/null +++ b/ur5e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur5_e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur5e_moveit_config/launch/ompl_planning_pipeline.launch.xml similarity index 90% rename from ur5_e_moveit_config/launch/ompl_planning_pipeline.launch.xml rename to ur5e_moveit_config/launch/ompl_planning_pipeline.launch.xml index eb0ef8926..7ec6880f1 100644 --- a/ur5_e_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur5e_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - + diff --git a/ur5_e_moveit_config/launch/planning_context.launch b/ur5e_moveit_config/launch/planning_context.launch similarity index 55% rename from ur5_e_moveit_config/launch/planning_context.launch rename to ur5e_moveit_config/launch/planning_context.launch index 63109be0a..9052fd96f 100644 --- a/ur5_e_moveit_config/launch/planning_context.launch +++ b/ur5e_moveit_config/launch/planning_context.launch @@ -1,28 +1,25 @@ - - - - - + - - + + - + - + + - + diff --git a/ur5_e_moveit_config/launch/planning_pipeline.launch.xml b/ur5e_moveit_config/launch/planning_pipeline.launch.xml similarity index 57% rename from ur5_e_moveit_config/launch/planning_pipeline.launch.xml rename to ur5e_moveit_config/launch/planning_pipeline.launch.xml index 12211f9d8..caf1be307 100644 --- a/ur5_e_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur5e_moveit_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/ur5e_moveit_config/launch/ros_controllers.launch b/ur5e_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..d71e18a0d --- /dev/null +++ b/ur5e_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur5_e_moveit_config/launch/run_benchmark_ompl.launch b/ur5e_moveit_config/launch/run_benchmark_ompl.launch similarity index 63% rename from ur5_e_moveit_config/launch/run_benchmark_ompl.launch rename to ur5e_moveit_config/launch/run_benchmark_ompl.launch index d37513d82..757bc9c52 100644 --- a/ur5_e_moveit_config/launch/run_benchmark_ompl.launch +++ b/ur5e_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/ur5_e_moveit_config/launch/sensor_manager.launch.xml b/ur5e_moveit_config/launch/sensor_manager.launch.xml similarity index 73% rename from ur5_e_moveit_config/launch/sensor_manager.launch.xml rename to ur5e_moveit_config/launch/sensor_manager.launch.xml index 172eb87be..97df2bd8f 100644 --- a/ur5_e_moveit_config/launch/sensor_manager.launch.xml +++ b/ur5e_moveit_config/launch/sensor_manager.launch.xml @@ -8,7 +8,7 @@ - - + + diff --git a/ur5_e_moveit_config/launch/setup_assistant.launch b/ur5e_moveit_config/launch/setup_assistant.launch similarity index 60% rename from ur5_e_moveit_config/launch/setup_assistant.launch rename to ur5e_moveit_config/launch/setup_assistant.launch index 3d712d563..6e0054587 100644 --- a/ur5_e_moveit_config/launch/setup_assistant.launch +++ b/ur5e_moveit_config/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -7,9 +7,9 @@ - diff --git a/ur5_e_moveit_config/launch/trajectory_execution.launch.xml b/ur5e_moveit_config/launch/trajectory_execution.launch.xml similarity index 80% rename from ur5_e_moveit_config/launch/trajectory_execution.launch.xml rename to ur5e_moveit_config/launch/trajectory_execution.launch.xml index 3513753a6..bbad585bf 100644 --- a/ur5_e_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur5e_moveit_config/launch/trajectory_execution.launch.xml @@ -1,6 +1,6 @@ - + @@ -10,9 +10,9 @@ - + - - - + + + diff --git a/ur5_e_moveit_config/launch/ur5_e_moveit_controller_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_moveit_controller_manager.launch.xml similarity index 78% rename from ur5_e_moveit_config/launch/ur5_e_moveit_controller_manager.launch.xml rename to ur5e_moveit_config/launch/ur5e_moveit_controller_manager.launch.xml index 6bc194070..5558a06b9 100644 --- a/ur5_e_moveit_config/launch/ur5_e_moveit_controller_manager.launch.xml +++ b/ur5e_moveit_config/launch/ur5e_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur5_e_moveit_config/launch/ur5_e_moveit_planning_execution.launch b/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch similarity index 65% rename from ur5_e_moveit_config/launch/ur5_e_moveit_planning_execution.launch rename to ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch index 148c40e4b..e73b32cc3 100644 --- a/ur5_e_moveit_config/launch/ur5_e_moveit_planning_execution.launch +++ b/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - + - - + diff --git a/ur5_e_moveit_config/launch/ur5_e_moveit_sensor_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_moveit_sensor_manager.launch.xml similarity index 100% rename from ur5_e_moveit_config/launch/ur5_e_moveit_sensor_manager.launch.xml rename to ur5e_moveit_config/launch/ur5e_moveit_sensor_manager.launch.xml diff --git a/ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..58e68a4ab --- /dev/null +++ b/ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur5_e_moveit_config/launch/warehouse.launch b/ur5e_moveit_config/launch/warehouse.launch similarity index 67% rename from ur5_e_moveit_config/launch/warehouse.launch rename to ur5e_moveit_config/launch/warehouse.launch index 3ac1ddf6a..4ec8b7786 100644 --- a/ur5_e_moveit_config/launch/warehouse.launch +++ b/ur5e_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - - + + - + diff --git a/ur5_e_moveit_config/launch/warehouse_settings.launch.xml b/ur5e_moveit_config/launch/warehouse_settings.launch.xml similarity index 64% rename from ur5_e_moveit_config/launch/warehouse_settings.launch.xml rename to ur5e_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur5_e_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur5e_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur5_e_moveit_config/package.xml b/ur5e_moveit_config/package.xml similarity index 94% rename from ur5_e_moveit_config/package.xml rename to ur5e_moveit_config/package.xml index f81c4cc1d..f1087d6e0 100644 --- a/ur5_e_moveit_config/package.xml +++ b/ur5e_moveit_config/package.xml @@ -1,7 +1,7 @@ - ur5_e_moveit_config + ur5e_moveit_config 1.2.5 An automatically generated package with all the configuration and launch files for using the ur5e with the MoveIt Motion Planning Framework @@ -10,7 +10,7 @@ G.A. vd. Hoorn Miguel Prada Sarasola Nadia Hammoudeh Garcia - + BSD http://moveit.ros.org/ @@ -24,10 +24,11 @@ moveit_simple_controller_manager joint_state_publisher robot_state_publisher + tf2_ros xacro ur_description roslaunch catkin - + diff --git a/ur5e_moveit_config/tests/moveit_planning_execution.xml b/ur5e_moveit_config/tests/moveit_planning_execution.xml new file mode 100644 index 000000000..34976b996 --- /dev/null +++ b/ur5e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + From 3f8a18a58bd3744e4d9701c98eff334abe6866e1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 13:50:57 +0100 Subject: [PATCH 08/28] Added ur16e moveit config --- universal_robots/package.xml | 1 + ur16e_moveit_config/.setup_assistant | 11 + ur16e_moveit_config/CMakeLists.txt | 14 + .../config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 15 + ur16e_moveit_config/config/joint_limits.yaml | 34 + ur16e_moveit_config/config/kinematics.yaml | 10 + ur16e_moveit_config/config/ompl_planning.yaml | 172 +++++ .../config/ros_controllers.yaml | 34 + ur16e_moveit_config/config/ur16e.srdf | 51 ++ .../launch/chomp_planning_pipeline.launch.xml | 20 + .../launch/default_warehouse_db.launch | 15 + ur16e_moveit_config/launch/demo.launch | 63 ++ .../fake_moveit_controller_manager.launch.xml | 9 + ur16e_moveit_config/launch/move_group.launch | 82 +++ ur16e_moveit_config/launch/moveit.rviz | 689 ++++++++++++++++++ ur16e_moveit_config/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 22 + .../launch/planning_context.launch | 25 + .../launch/planning_pipeline.launch.xml | 10 + .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 22 + .../launch/sensor_manager.launch.xml | 14 + .../launch/setup_assistant.launch | 15 + .../launch/trajectory_execution.launch.xml | 18 + ...ur16e_moveit_controller_manager.launch.xml | 6 + .../ur16e_moveit_planning_execution.launch | 12 + .../ur16e_moveit_sensor_manager.launch.xml | 3 + ...robot_moveit_controller_manager.launch.xml | 10 + ...16e_robot_moveit_sensor_manager.launch.xml | 3 + ur16e_moveit_config/launch/warehouse.launch | 15 + .../launch/warehouse_settings.launch.xml | 16 + ur16e_moveit_config/package.xml | 34 + .../tests/moveit_planning_execution.xml | 16 + 34 files changed, 1505 insertions(+) create mode 100644 ur16e_moveit_config/.setup_assistant create mode 100644 ur16e_moveit_config/CMakeLists.txt create mode 100644 ur16e_moveit_config/config/chomp_planning.yaml create mode 100644 ur16e_moveit_config/config/fake_controllers.yaml create mode 100644 ur16e_moveit_config/config/joint_limits.yaml create mode 100644 ur16e_moveit_config/config/kinematics.yaml create mode 100644 ur16e_moveit_config/config/ompl_planning.yaml create mode 100644 ur16e_moveit_config/config/ros_controllers.yaml create mode 100644 ur16e_moveit_config/config/ur16e.srdf create mode 100644 ur16e_moveit_config/launch/chomp_planning_pipeline.launch.xml create mode 100644 ur16e_moveit_config/launch/default_warehouse_db.launch create mode 100644 ur16e_moveit_config/launch/demo.launch create mode 100644 ur16e_moveit_config/launch/fake_moveit_controller_manager.launch.xml create mode 100644 ur16e_moveit_config/launch/move_group.launch create mode 100644 ur16e_moveit_config/launch/moveit.rviz create mode 100644 ur16e_moveit_config/launch/moveit_rviz.launch create mode 100644 ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml create mode 100644 ur16e_moveit_config/launch/planning_context.launch create mode 100644 ur16e_moveit_config/launch/planning_pipeline.launch.xml create mode 100644 ur16e_moveit_config/launch/ros_controllers.launch create mode 100644 ur16e_moveit_config/launch/run_benchmark_ompl.launch create mode 100644 ur16e_moveit_config/launch/sensor_manager.launch.xml create mode 100644 ur16e_moveit_config/launch/setup_assistant.launch create mode 100644 ur16e_moveit_config/launch/trajectory_execution.launch.xml create mode 100644 ur16e_moveit_config/launch/ur16e_moveit_controller_manager.launch.xml create mode 100644 ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch create mode 100644 ur16e_moveit_config/launch/ur16e_moveit_sensor_manager.launch.xml create mode 100644 ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml create mode 100644 ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml create mode 100644 ur16e_moveit_config/launch/warehouse.launch create mode 100644 ur16e_moveit_config/launch/warehouse_settings.launch.xml create mode 100644 ur16e_moveit_config/package.xml create mode 100644 ur16e_moveit_config/tests/moveit_planning_execution.xml diff --git a/universal_robots/package.xml b/universal_robots/package.xml index 0593da5df..02be64f71 100644 --- a/universal_robots/package.xml +++ b/universal_robots/package.xml @@ -22,6 +22,7 @@ ur10e_moveit_config ur10_moveit_config + ur16e_moveit_config ur3e_moveit_config ur3_moveit_config ur5e_moveit_config diff --git a/ur16e_moveit_config/.setup_assistant b/ur16e_moveit_config/.setup_assistant new file mode 100644 index 000000000..aa8aaae2c --- /dev/null +++ b/ur16e_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: ur_description + relative_path: urdf/ur16e.xacro + xacro_args: "" + SRDF: + relative_path: config/ur16e.srdf + CONFIG: + author_name: Felix Exner + author_email: felix@fexner.de + generated_timestamp: 1611154369 \ No newline at end of file diff --git a/ur16e_moveit_config/CMakeLists.txt b/ur16e_moveit_config/CMakeLists.txt new file mode 100644 index 000000000..09769e2be --- /dev/null +++ b/ur16e_moveit_config/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ur16e_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +if (CATKIN_ENABLE_TESTING) + find_package(roslaunch REQUIRED) + roslaunch_add_file_check(tests/moveit_planning_execution.xml) +endif() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/ur16e_moveit_config/config/chomp_planning.yaml b/ur16e_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..75258e53e --- /dev/null +++ b/ur16e_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/ur16e_moveit_config/config/fake_controllers.yaml b/ur16e_moveit_config/config/fake_controllers.yaml new file mode 100644 index 000000000..e328e94d4 --- /dev/null +++ b/ur16e_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,15 @@ +controller_list: + - name: fake_manipulator_controller + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + - name: fake_endeffector_controller + joints: + [] +initial: # Define initial robot poses. + - group: manipulator + pose: home \ No newline at end of file diff --git a/ur16e_moveit_config/config/joint_limits.yaml b/ur16e_moveit_config/config/joint_limits.yaml new file mode 100644 index 000000000..633b7bbce --- /dev/null +++ b/ur16e_moveit_config/config/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + elbow_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_lift_joint: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: false + max_acceleration: 0 + wrist_1_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + wrist_2_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + wrist_3_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/ur16e_moveit_config/config/kinematics.yaml b/ur16e_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..e37d336c3 --- /dev/null +++ b/ur16e_moveit_config/config/kinematics.yaml @@ -0,0 +1,10 @@ +#manipulator: +# kinematics_solver: ur_kinematics/UR10KinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 +manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/ur16e_moveit_config/config/ompl_planning.yaml b/ur16e_moveit_config/config/ompl_planning.yaml new file mode 100644 index 000000000..acbf6bd9b --- /dev/null +++ b/ur16e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,172 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + 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 + LBKPIECE: + 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 + BKPIECE: + 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 + KPIECE: + 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 + RRT: + 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 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + 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 + TRRT: + 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 expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + 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 + BFMT: + 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 + PDST: + type: geometric::PDST + STRIDE: + 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 + BiTRRT: + 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 + LBTRRT: + 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 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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 + SPARStwo: + 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 +manipulator: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +endeffector: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/ur16e_moveit_config/config/ros_controllers.yaml b/ur16e_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..0ecf90959 --- /dev/null +++ b/ur16e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,34 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: manipulator + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: +- name: "scaled_pos_joint_traj_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur16e_moveit_config/config/ur16e.srdf b/ur16e_moveit_config/config/ur16e.srdf new file mode 100644 index 000000000..5dc0b2e00 --- /dev/null +++ b/ur16e_moveit_config/config/ur16e.srdf @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/chomp_planning_pipeline.launch.xml b/ur16e_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..0a3093def --- /dev/null +++ b/ur16e_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/default_warehouse_db.launch b/ur16e_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 000000000..659763848 --- /dev/null +++ b/ur16e_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/demo.launch b/ur16e_moveit_config/launch/demo.launch new file mode 100644 index 000000000..6d8e7ab8c --- /dev/null +++ b/ur16e_moveit_config/launch/demo.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur16e_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..031184d30 --- /dev/null +++ b/ur16e_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/ur16e_moveit_config/launch/move_group.launch b/ur16e_moveit_config/launch/move_group.launch new file mode 100644 index 000000000..9418b0939 --- /dev/null +++ b/ur16e_moveit_config/launch/move_group.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/moveit.rviz b/ur16e_moveit_config/launch/moveit.rviz new file mode 100644 index 000000000..b431b2a03 --- /dev/null +++ b/ur16e_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/ur16e_moveit_config/launch/moveit_rviz.launch b/ur16e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..a4605c037 --- /dev/null +++ b/ur16e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 000000000..0ac9828c4 --- /dev/null +++ b/ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/planning_context.launch b/ur16e_moveit_config/launch/planning_context.launch new file mode 100644 index 000000000..2c61f888c --- /dev/null +++ b/ur16e_moveit_config/launch/planning_context.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/planning_pipeline.launch.xml b/ur16e_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 000000000..fc6e0f94c --- /dev/null +++ b/ur16e_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur16e_moveit_config/launch/ros_controllers.launch b/ur16e_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..7789e42dd --- /dev/null +++ b/ur16e_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/run_benchmark_ompl.launch b/ur16e_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 000000000..40e7444e6 --- /dev/null +++ b/ur16e_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/sensor_manager.launch.xml b/ur16e_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 000000000..a8db6fdb8 --- /dev/null +++ b/ur16e_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/setup_assistant.launch b/ur16e_moveit_config/launch/setup_assistant.launch new file mode 100644 index 000000000..385ea95aa --- /dev/null +++ b/ur16e_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/trajectory_execution.launch.xml b/ur16e_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 000000000..099f3c9fe --- /dev/null +++ b/ur16e_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/ur16e_moveit_controller_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..0aab1122f --- /dev/null +++ b/ur16e_moveit_config/launch/ur16e_moveit_controller_manager.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch b/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch new file mode 100644 index 000000000..588ea4684 --- /dev/null +++ b/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/ur16e_moveit_sensor_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur16e_moveit_config/launch/ur16e_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..0a65168d1 --- /dev/null +++ b/ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur16e_moveit_config/launch/warehouse.launch b/ur16e_moveit_config/launch/warehouse.launch new file mode 100644 index 000000000..529d5e2a6 --- /dev/null +++ b/ur16e_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/warehouse_settings.launch.xml b/ur16e_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 000000000..e473b083b --- /dev/null +++ b/ur16e_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/ur16e_moveit_config/package.xml b/ur16e_moveit_config/package.xml new file mode 100644 index 000000000..d0cdb1026 --- /dev/null +++ b/ur16e_moveit_config/package.xml @@ -0,0 +1,34 @@ + + + + ur16e_moveit_config + 1.2.5 + + An automatically generated package with all the configuration and launch files for using the ur16e with the MoveIt Motion Planning Framework + + Felix Messmer + G.A. vd. Hoorn + Miguel Prada Sarasola + Nadia Hammoudeh Garcia + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit_setup_assistant/issues + https://github.com/ros-planning/moveit_setup_assistant + + moveit_ros_move_group + moveit_planners_ompl + moveit_ros_visualization + moveit_fake_controller_manager + moveit_simple_controller_manager + joint_state_publisher + robot_state_publisher + tf2_ros + xacro + ur_description + + roslaunch + catkin + + diff --git a/ur16e_moveit_config/tests/moveit_planning_execution.xml b/ur16e_moveit_config/tests/moveit_planning_execution.xml new file mode 100644 index 000000000..f7a2c9d9f --- /dev/null +++ b/ur16e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + From d2c399cd33945e25d1952234cebaf5435d6c64ef Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 15:21:34 +0100 Subject: [PATCH 09/28] Updated remap to default controller --- ur10_moveit_config/launch/ur10_moveit_planning_execution.launch | 2 +- ur10_moveit_config/tests/moveit_planning_execution.xml | 2 +- .../launch/ur10e_moveit_planning_execution.launch | 2 +- ur10e_moveit_config/tests/moveit_planning_execution.xml | 2 +- .../launch/ur16e_moveit_planning_execution.launch | 2 +- ur16e_moveit_config/tests/moveit_planning_execution.xml | 2 +- ur3_moveit_config/launch/ur3_moveit_planning_execution.launch | 2 +- ur3_moveit_config/tests/moveit_planning_execution.xml | 2 +- ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch | 2 +- ur3e_moveit_config/tests/moveit_planning_execution.xml | 2 +- ur5_moveit_config/launch/ur5_moveit_planning_execution.launch | 2 +- ur5_moveit_config/tests/moveit_planning_execution.xml | 2 +- ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch | 2 +- ur5e_moveit_config/tests/moveit_planning_execution.xml | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch b/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch index 596ecb835..7ee715dd6 100644 --- a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch +++ b/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur10_moveit_config/tests/moveit_planning_execution.xml b/ur10_moveit_config/tests/moveit_planning_execution.xml index 0d5f8c672..843f2bece 100644 --- a/ur10_moveit_config/tests/moveit_planning_execution.xml +++ b/ur10_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + diff --git a/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch b/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch index e7240f802..cb6f739b2 100644 --- a/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch +++ b/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur10e_moveit_config/tests/moveit_planning_execution.xml b/ur10e_moveit_config/tests/moveit_planning_execution.xml index 910165b8b..fe138e5f1 100644 --- a/ur10e_moveit_config/tests/moveit_planning_execution.xml +++ b/ur10e_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + diff --git a/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch b/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch index 588ea4684..e88ac0437 100644 --- a/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch +++ b/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur16e_moveit_config/tests/moveit_planning_execution.xml b/ur16e_moveit_config/tests/moveit_planning_execution.xml index f7a2c9d9f..3ff79057b 100644 --- a/ur16e_moveit_config/tests/moveit_planning_execution.xml +++ b/ur16e_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + diff --git a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch b/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch index 2631567cc..53c114e13 100644 --- a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch +++ b/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur3_moveit_config/tests/moveit_planning_execution.xml b/ur3_moveit_config/tests/moveit_planning_execution.xml index 3c6c18d7f..10246c7d7 100644 --- a/ur3_moveit_config/tests/moveit_planning_execution.xml +++ b/ur3_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + diff --git a/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch b/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch index 91de984ad..59da97306 100644 --- a/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch +++ b/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur3e_moveit_config/tests/moveit_planning_execution.xml b/ur3e_moveit_config/tests/moveit_planning_execution.xml index 81c4302af..6025d2427 100644 --- a/ur3e_moveit_config/tests/moveit_planning_execution.xml +++ b/ur3e_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + diff --git a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch b/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch index e1c225159..5648324c1 100644 --- a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch +++ b/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur5_moveit_config/tests/moveit_planning_execution.xml b/ur5_moveit_config/tests/moveit_planning_execution.xml index e3f3ae0ad..16052aa74 100644 --- a/ur5_moveit_config/tests/moveit_planning_execution.xml +++ b/ur5_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + diff --git a/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch b/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch index e73b32cc3..acd7ef8b8 100644 --- a/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch +++ b/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur5e_moveit_config/tests/moveit_planning_execution.xml b/ur5e_moveit_config/tests/moveit_planning_execution.xml index 34976b996..1a256e673 100644 --- a/ur5e_moveit_config/tests/moveit_planning_execution.xml +++ b/ur5e_moveit_config/tests/moveit_planning_execution.xml @@ -7,7 +7,7 @@ - + From bc0ce06f3dd4fa5f856adf9fde64d9848456ff6b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Mar 2021 15:48:43 +0100 Subject: [PATCH 10/28] Updated joint speed limits --- ur3_moveit_config/config/joint_limits.yaml | 12 ++++++------ ur3e_moveit_config/config/joint_limits.yaml | 12 ++++++------ ur5_moveit_config/config/joint_limits.yaml | 6 +++--- ur5e_moveit_config/config/joint_limits.yaml | 6 +++--- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/ur3_moveit_config/config/joint_limits.yaml b/ur3_moveit_config/config/joint_limits.yaml index 633b7bbce..45694aa56 100644 --- a/ur3_moveit_config/config/joint_limits.yaml +++ b/ur3_moveit_config/config/joint_limits.yaml @@ -9,26 +9,26 @@ joint_limits: max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 shoulder_pan_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 6.283185307179586 has_acceleration_limits: false max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 6.283185307179586 has_acceleration_limits: false max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 6.283185307179586 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_acceleration: 0 diff --git a/ur3e_moveit_config/config/joint_limits.yaml b/ur3e_moveit_config/config/joint_limits.yaml index 633b7bbce..45694aa56 100644 --- a/ur3e_moveit_config/config/joint_limits.yaml +++ b/ur3e_moveit_config/config/joint_limits.yaml @@ -9,26 +9,26 @@ joint_limits: max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 shoulder_pan_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 wrist_1_joint: has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 6.283185307179586 has_acceleration_limits: false max_acceleration: 0 wrist_2_joint: has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 6.283185307179586 has_acceleration_limits: false max_acceleration: 0 wrist_3_joint: has_velocity_limits: true - max_velocity: 3.1415926535897931 + max_velocity: 6.283185307179586 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_acceleration: 0 diff --git a/ur5_moveit_config/config/joint_limits.yaml b/ur5_moveit_config/config/joint_limits.yaml index 633b7bbce..bdc914f89 100644 --- a/ur5_moveit_config/config/joint_limits.yaml +++ b/ur5_moveit_config/config/joint_limits.yaml @@ -9,12 +9,12 @@ joint_limits: max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 shoulder_pan_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 wrist_1_joint: @@ -31,4 +31,4 @@ joint_limits: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_acceleration: 0 diff --git a/ur5e_moveit_config/config/joint_limits.yaml b/ur5e_moveit_config/config/joint_limits.yaml index 633b7bbce..bdc914f89 100644 --- a/ur5e_moveit_config/config/joint_limits.yaml +++ b/ur5e_moveit_config/config/joint_limits.yaml @@ -9,12 +9,12 @@ joint_limits: max_acceleration: 0 shoulder_lift_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 shoulder_pan_joint: has_velocity_limits: true - max_velocity: 2.0943951023931953 + max_velocity: 3.1415926535897931 has_acceleration_limits: false max_acceleration: 0 wrist_1_joint: @@ -31,4 +31,4 @@ joint_limits: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_acceleration: 0 From 25e20a4efe02e1a42700b82bdac64e6b99cf67d0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 30 Jun 2021 11:23:47 +0200 Subject: [PATCH 11/28] Correct remapped controller name in sim mode --- ur10_moveit_config/launch/ur10_moveit_planning_execution.launch | 2 +- .../launch/ur10e_moveit_planning_execution.launch | 2 +- .../launch/ur16e_moveit_planning_execution.launch | 2 +- ur3_moveit_config/launch/ur3_moveit_planning_execution.launch | 2 +- ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch | 2 +- ur5_moveit_config/launch/ur5_moveit_planning_execution.launch | 2 +- ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch b/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch index 7ee715dd6..80f728f0e 100644 --- a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch +++ b/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch b/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch index cb6f739b2..de9c149f5 100644 --- a/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch +++ b/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch b/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch index e88ac0437..e3c2882f0 100644 --- a/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch +++ b/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch b/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch index 53c114e13..3c97229dd 100644 --- a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch +++ b/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch b/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch index 59da97306..939c0769b 100644 --- a/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch +++ b/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch b/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch index 5648324c1..3022d867d 100644 --- a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch +++ b/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + diff --git a/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch b/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch index acd7ef8b8..82a6e2105 100644 --- a/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch +++ b/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch @@ -3,7 +3,7 @@ - + From e4a6bfd4155c2fb2efd7b60de96e95906082be23 Mon Sep 17 00:00:00 2001 From: Luke Dennis Date: Sat, 10 Jul 2021 14:59:10 +1000 Subject: [PATCH 12/28] Delete ur5_e_moveit_config directory Seems to be an artefact missed in the change over --- ur5_e_moveit_config/CHANGELOG.rst | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 ur5_e_moveit_config/CHANGELOG.rst diff --git a/ur5_e_moveit_config/CHANGELOG.rst b/ur5_e_moveit_config/CHANGELOG.rst deleted file mode 100644 index 6556da705..000000000 --- a/ur5_e_moveit_config/CHANGELOG.rst +++ /dev/null @@ -1,10 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ur5_e_moveit_config -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.5 (2019-04-05) ------------------- -* First release (of this package) -* Update maintainer listing: add Miguel (`#410 `_) -* UR-E Series (`#380 `_) -* Contributors: Dave Niewinski, gavanderhoorn From 2c18d92abd4a2723949204fdcde7fe3c077d3628 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 22 Jul 2021 14:43:51 +0200 Subject: [PATCH 13/28] Set the default planning group to manipulator --- ur10_moveit_config/launch/moveit.rviz | 2 +- ur10e_moveit_config/launch/moveit.rviz | 2 +- ur16e_moveit_config/launch/moveit.rviz | 2 +- ur3_moveit_config/launch/moveit.rviz | 2 +- ur3e_moveit_config/launch/moveit.rviz | 2 +- ur5_moveit_config/launch/moveit.rviz | 2 +- ur5e_moveit_config/launch/moveit.rviz | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ur10_moveit_config/launch/moveit.rviz b/ur10_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur10_moveit_config/launch/moveit.rviz +++ b/ur10_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/ur10e_moveit_config/launch/moveit.rviz b/ur10e_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur10e_moveit_config/launch/moveit.rviz +++ b/ur10e_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/ur16e_moveit_config/launch/moveit.rviz b/ur16e_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur16e_moveit_config/launch/moveit.rviz +++ b/ur16e_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/ur3_moveit_config/launch/moveit.rviz b/ur3_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur3_moveit_config/launch/moveit.rviz +++ b/ur3_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/ur3e_moveit_config/launch/moveit.rviz b/ur3e_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur3e_moveit_config/launch/moveit.rviz +++ b/ur3e_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/ur5_moveit_config/launch/moveit.rviz b/ur5_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur5_moveit_config/launch/moveit.rviz +++ b/ur5_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false diff --git a/ur5e_moveit_config/launch/moveit.rviz b/ur5e_moveit_config/launch/moveit.rviz index b431b2a03..33d044209 100644 --- a/ur5e_moveit_config/launch/moveit.rviz +++ b/ur5e_moveit_config/launch/moveit.rviz @@ -344,7 +344,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: manipulator Query Goal State: true Query Start State: false Show Workspace: false From a445e76f888b984dae4b545814a771d018b24e1a Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 13:34:28 +0200 Subject: [PATCH 14/28] moveit: switch to Trac IK everywhere Better performance than KDL, more precise, more predictable and still compatible with the calibrated URDF. --- ur10_moveit_config/config/kinematics.yaml | 3 ++- ur10_moveit_config/package.xml | 3 ++- ur10e_moveit_config/config/kinematics.yaml | 3 ++- ur10e_moveit_config/package.xml | 3 ++- ur16e_moveit_config/config/kinematics.yaml | 3 ++- ur16e_moveit_config/package.xml | 3 ++- ur3_moveit_config/config/kinematics.yaml | 3 ++- ur3_moveit_config/package.xml | 3 ++- ur3e_moveit_config/config/kinematics.yaml | 3 ++- ur3e_moveit_config/package.xml | 3 ++- ur5_moveit_config/config/kinematics.yaml | 3 ++- ur5_moveit_config/package.xml | 3 ++- ur5e_moveit_config/config/kinematics.yaml | 3 ++- ur5e_moveit_config/package.xml | 3 ++- 14 files changed, 28 insertions(+), 14 deletions(-) diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur10_moveit_config/config/kinematics.yaml +++ b/ur10_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur10_moveit_config/package.xml b/ur10_moveit_config/package.xml index cc1ab542a..6eab01629 100644 --- a/ur10_moveit_config/package.xml +++ b/ur10_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin diff --git a/ur10e_moveit_config/config/kinematics.yaml b/ur10e_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur10e_moveit_config/config/kinematics.yaml +++ b/ur10e_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur10e_moveit_config/package.xml b/ur10e_moveit_config/package.xml index c1f63a751..9f32bbb42 100644 --- a/ur10e_moveit_config/package.xml +++ b/ur10e_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin diff --git a/ur16e_moveit_config/config/kinematics.yaml b/ur16e_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur16e_moveit_config/config/kinematics.yaml +++ b/ur16e_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur16e_moveit_config/package.xml b/ur16e_moveit_config/package.xml index d0cdb1026..fe5b33521 100644 --- a/ur16e_moveit_config/package.xml +++ b/ur16e_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin diff --git a/ur3_moveit_config/config/kinematics.yaml b/ur3_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur3_moveit_config/config/kinematics.yaml +++ b/ur3_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur3_moveit_config/package.xml b/ur3_moveit_config/package.xml index 67301f293..73738e439 100644 --- a/ur3_moveit_config/package.xml +++ b/ur3_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin diff --git a/ur3e_moveit_config/config/kinematics.yaml b/ur3e_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur3e_moveit_config/config/kinematics.yaml +++ b/ur3e_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur3e_moveit_config/package.xml b/ur3e_moveit_config/package.xml index 4563bfde3..36692086f 100644 --- a/ur3e_moveit_config/package.xml +++ b/ur3e_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur5_moveit_config/config/kinematics.yaml +++ b/ur5_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur5_moveit_config/package.xml b/ur5_moveit_config/package.xml index a2d2e00a9..160034ba8 100644 --- a/ur5_moveit_config/package.xml +++ b/ur5_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin diff --git a/ur5e_moveit_config/config/kinematics.yaml b/ur5e_moveit_config/config/kinematics.yaml index e37d336c3..93972fd64 100644 --- a/ur5e_moveit_config/config/kinematics.yaml +++ b/ur5e_moveit_config/config/kinematics.yaml @@ -4,7 +4,8 @@ # kinematics_solver_timeout: 0.005 # kinematics_solver_attempts: 3 manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 diff --git a/ur5e_moveit_config/package.xml b/ur5e_moveit_config/package.xml index f1087d6e0..98ab9abd6 100644 --- a/ur5e_moveit_config/package.xml +++ b/ur5e_moveit_config/package.xml @@ -30,5 +30,6 @@ roslaunch catkin - + + trac_ik_kinematics_plugin From 9c516d8a76268fde975290e5badf2613f91ae95a Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 16:09:32 +0200 Subject: [PATCH 15/28] moveit: solver_attempts is no longer supported --- ur10_moveit_config/config/kinematics.yaml | 1 - ur10e_moveit_config/config/kinematics.yaml | 1 - ur16e_moveit_config/config/kinematics.yaml | 1 - ur3_moveit_config/config/kinematics.yaml | 1 - ur3e_moveit_config/config/kinematics.yaml | 1 - ur5_moveit_config/config/kinematics.yaml | 1 - ur5e_moveit_config/config/kinematics.yaml | 1 - 7 files changed, 7 deletions(-) diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur10_moveit_config/config/kinematics.yaml +++ b/ur10_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/ur10e_moveit_config/config/kinematics.yaml b/ur10e_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur10e_moveit_config/config/kinematics.yaml +++ b/ur10e_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/ur16e_moveit_config/config/kinematics.yaml b/ur16e_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur16e_moveit_config/config/kinematics.yaml +++ b/ur16e_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/ur3_moveit_config/config/kinematics.yaml b/ur3_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur3_moveit_config/config/kinematics.yaml +++ b/ur3_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/ur3e_moveit_config/config/kinematics.yaml b/ur3e_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur3e_moveit_config/config/kinematics.yaml +++ b/ur3e_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur5_moveit_config/config/kinematics.yaml +++ b/ur5_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/ur5e_moveit_config/config/kinematics.yaml b/ur5e_moveit_config/config/kinematics.yaml index 93972fd64..51e29f53e 100644 --- a/ur5e_moveit_config/config/kinematics.yaml +++ b/ur5e_moveit_config/config/kinematics.yaml @@ -8,4 +8,3 @@ manipulator: solve_type: Distance kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 From 517e4272a64e70d5c6178ffd8dd93d71dfa5d569 Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 16:10:46 +0200 Subject: [PATCH 16/28] moveit: use a saner RViz config --- ur10_moveit_config/launch/moveit.rviz | 593 ++++--------------------- ur10e_moveit_config/launch/moveit.rviz | 593 ++++--------------------- ur16e_moveit_config/launch/moveit.rviz | 593 ++++--------------------- ur3_moveit_config/launch/moveit.rviz | 593 ++++--------------------- ur3e_moveit_config/launch/moveit.rviz | 593 ++++--------------------- ur5_moveit_config/launch/moveit.rviz | 593 ++++--------------------- ur5e_moveit_config/launch/moveit.rviz | 593 ++++--------------------- 7 files changed, 644 insertions(+), 3507 deletions(-) diff --git a/ur10_moveit_config/launch/moveit.rviz b/ur10_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur10_moveit_config/launch/moveit.rviz +++ b/ur10_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 diff --git a/ur10e_moveit_config/launch/moveit.rviz b/ur10e_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur10e_moveit_config/launch/moveit.rviz +++ b/ur10e_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 diff --git a/ur16e_moveit_config/launch/moveit.rviz b/ur16e_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur16e_moveit_config/launch/moveit.rviz +++ b/ur16e_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 diff --git a/ur3_moveit_config/launch/moveit.rviz b/ur3_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur3_moveit_config/launch/moveit.rviz +++ b/ur3_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 diff --git a/ur3e_moveit_config/launch/moveit.rviz b/ur3e_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur3e_moveit_config/launch/moveit.rviz +++ b/ur3e_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 diff --git a/ur5_moveit_config/launch/moveit.rviz b/ur5_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur5_moveit_config/launch/moveit.rviz +++ b/ur5_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 diff --git a/ur5e_moveit_config/launch/moveit.rviz b/ur5e_moveit_config/launch/moveit.rviz index 33d044209..cfc2fec04 100644 --- a/ur5e_moveit_config/launch/moveit.rviz +++ b/ur5e_moveit_config/launch/moveit.rviz @@ -1,12 +1,12 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,98 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - MoveIt_Goal_Tolerance: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_gripper_r_finger_tip_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_shoulder_lift_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_shoulder_pan_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - l_upper_arm_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_l_finger_tip_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_motor_accelerometer_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,6 +138,7 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 @@ -355,300 +156,80 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: Alpha: 1 Show Axes: false Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - bl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_rotation_link: + flange: Alpha: 1 Show Axes: false Show Trail: false - Value: true - double_stereo_link: + forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_l_wheel_link: + shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fl_caster_r_wheel_link: + tool0: Alpha: 1 Show Axes: false Show Trail: false - Value: true - fl_caster_rotation_link: + upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_l_wheel_link: + wrist_1_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_r_wheel_link: + wrist_2_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist_3_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +240,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 1.1539382934570312 + Y: 0.5978575348854065 + Z: -2.5331917186122155e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 5.65995 + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 783 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1182 + X: 481 + Y: 253 From 49a4fee481d2d3735a8aa359c115696b3586faba Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 16:14:35 +0200 Subject: [PATCH 17/28] moveit: ros_control is managed by the driver So this file is not used here. --- ur10_moveit_config/launch/ros_controllers.launch | 11 ----------- ur10e_moveit_config/launch/ros_controllers.launch | 11 ----------- ur16e_moveit_config/launch/ros_controllers.launch | 11 ----------- ur3_moveit_config/launch/ros_controllers.launch | 11 ----------- ur3e_moveit_config/launch/ros_controllers.launch | 11 ----------- ur5_moveit_config/launch/ros_controllers.launch | 11 ----------- ur5e_moveit_config/launch/ros_controllers.launch | 11 ----------- 7 files changed, 77 deletions(-) delete mode 100644 ur10_moveit_config/launch/ros_controllers.launch delete mode 100644 ur10e_moveit_config/launch/ros_controllers.launch delete mode 100644 ur16e_moveit_config/launch/ros_controllers.launch delete mode 100644 ur3_moveit_config/launch/ros_controllers.launch delete mode 100644 ur3e_moveit_config/launch/ros_controllers.launch delete mode 100644 ur5_moveit_config/launch/ros_controllers.launch delete mode 100644 ur5e_moveit_config/launch/ros_controllers.launch diff --git a/ur10_moveit_config/launch/ros_controllers.launch b/ur10_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index 5546211f8..000000000 --- a/ur10_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/ur10e_moveit_config/launch/ros_controllers.launch b/ur10e_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index ec48b6bd6..000000000 --- a/ur10e_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/ur16e_moveit_config/launch/ros_controllers.launch b/ur16e_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index 7789e42dd..000000000 --- a/ur16e_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/ur3_moveit_config/launch/ros_controllers.launch b/ur3_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index c412631f6..000000000 --- a/ur3_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/ur3e_moveit_config/launch/ros_controllers.launch b/ur3e_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index b3e545009..000000000 --- a/ur3e_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/ur5_moveit_config/launch/ros_controllers.launch b/ur5_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index fb90fcc68..000000000 --- a/ur5_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/ur5e_moveit_config/launch/ros_controllers.launch b/ur5e_moveit_config/launch/ros_controllers.launch deleted file mode 100644 index d71e18a0d..000000000 --- a/ur5e_moveit_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - From 3377e3e0e730ba18d382bdc338a18f41db7bcdc1 Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 16:20:29 +0200 Subject: [PATCH 18/28] moveit: ros_control_boilerplate is not used So prune these settings. --- .../config/ros_controllers.yaml | 23 ------------------- .../config/ros_controllers.yaml | 23 ------------------- .../config/ros_controllers.yaml | 23 ------------------- ur3_moveit_config/config/ros_controllers.yaml | 23 ------------------- .../config/ros_controllers.yaml | 23 ------------------- ur5_moveit_config/config/ros_controllers.yaml | 23 ------------------- .../config/ros_controllers.yaml | 23 ------------------- 7 files changed, 161 deletions(-) diff --git a/ur10_moveit_config/config/ros_controllers.yaml b/ur10_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur10_moveit_config/config/ros_controllers.yaml +++ b/ur10_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory diff --git a/ur10e_moveit_config/config/ros_controllers.yaml b/ur10e_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur10e_moveit_config/config/ros_controllers.yaml +++ b/ur10e_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory diff --git a/ur16e_moveit_config/config/ros_controllers.yaml b/ur16e_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur16e_moveit_config/config/ros_controllers.yaml +++ b/ur16e_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory diff --git a/ur3_moveit_config/config/ros_controllers.yaml b/ur3_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur3_moveit_config/config/ros_controllers.yaml +++ b/ur3_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory diff --git a/ur3e_moveit_config/config/ros_controllers.yaml b/ur3e_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur3e_moveit_config/config/ros_controllers.yaml +++ b/ur3e_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory diff --git a/ur5_moveit_config/config/ros_controllers.yaml b/ur5_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur5_moveit_config/config/ros_controllers.yaml +++ b/ur5_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory diff --git a/ur5e_moveit_config/config/ros_controllers.yaml b/ur5e_moveit_config/config/ros_controllers.yaml index 0ecf90959..bfc4186c4 100644 --- a/ur5e_moveit_config/config/ros_controllers.yaml +++ b/ur5e_moveit_config/config/ros_controllers.yaml @@ -1,26 +1,3 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: manipulator - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 controller_list: - name: "scaled_pos_joint_traj_controller" action_ns: follow_joint_trajectory From e92a417d981987ff7f88f3fc4b79c5c982d23be6 Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 16:23:28 +0200 Subject: [PATCH 19/28] moveit: default to RRTConnect --- ur10_moveit_config/config/ompl_planning.yaml | 1 + ur10e_moveit_config/config/ompl_planning.yaml | 1 + ur16e_moveit_config/config/ompl_planning.yaml | 1 + ur3_moveit_config/config/ompl_planning.yaml | 1 + ur3e_moveit_config/config/ompl_planning.yaml | 1 + ur5_moveit_config/config/ompl_planning.yaml | 1 + ur5e_moveit_config/config/ompl_planning.yaml | 1 + 7 files changed, 7 insertions(+) diff --git a/ur10_moveit_config/config/ompl_planning.yaml b/ur10_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur10_moveit_config/config/ompl_planning.yaml +++ b/ur10_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST diff --git a/ur10e_moveit_config/config/ompl_planning.yaml b/ur10e_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur10e_moveit_config/config/ompl_planning.yaml +++ b/ur10e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST diff --git a/ur16e_moveit_config/config/ompl_planning.yaml b/ur16e_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur16e_moveit_config/config/ompl_planning.yaml +++ b/ur16e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST diff --git a/ur3_moveit_config/config/ompl_planning.yaml b/ur3_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur3_moveit_config/config/ompl_planning.yaml +++ b/ur3_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST diff --git a/ur3e_moveit_config/config/ompl_planning.yaml b/ur3e_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur3e_moveit_config/config/ompl_planning.yaml +++ b/ur3e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST diff --git a/ur5_moveit_config/config/ompl_planning.yaml b/ur5_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur5_moveit_config/config/ompl_planning.yaml +++ b/ur5_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST diff --git a/ur5e_moveit_config/config/ompl_planning.yaml b/ur5e_moveit_config/config/ompl_planning.yaml index acbf6bd9b..a1d28f721 100644 --- a/ur5e_moveit_config/config/ompl_planning.yaml +++ b/ur5e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + default_planner_config: RRTConnect planner_configs: - SBL - EST From 339e1ce11ca46ddce21f716532cf6e98629e801f Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 16:53:55 +0200 Subject: [PATCH 20/28] moveit: restore longest_valid_segment setting Unsure as to whether this is still needed, but let's keep what we had for now. --- ur10_moveit_config/config/ompl_planning.yaml | 1 + ur10e_moveit_config/config/ompl_planning.yaml | 1 + ur16e_moveit_config/config/ompl_planning.yaml | 1 + ur3_moveit_config/config/ompl_planning.yaml | 1 + ur3e_moveit_config/config/ompl_planning.yaml | 1 + ur5_moveit_config/config/ompl_planning.yaml | 1 + ur5e_moveit_config/config/ompl_planning.yaml | 1 + 7 files changed, 7 insertions(+) diff --git a/ur10_moveit_config/config/ompl_planning.yaml b/ur10_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur10_moveit_config/config/ompl_planning.yaml +++ b/ur10_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/ur10e_moveit_config/config/ompl_planning.yaml b/ur10e_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur10e_moveit_config/config/ompl_planning.yaml +++ b/ur10e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/ur16e_moveit_config/config/ompl_planning.yaml b/ur16e_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur16e_moveit_config/config/ompl_planning.yaml +++ b/ur16e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/ur3_moveit_config/config/ompl_planning.yaml b/ur3_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur3_moveit_config/config/ompl_planning.yaml +++ b/ur3_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/ur3e_moveit_config/config/ompl_planning.yaml b/ur3e_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur3e_moveit_config/config/ompl_planning.yaml +++ b/ur3e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/ur5_moveit_config/config/ompl_planning.yaml b/ur5_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur5_moveit_config/config/ompl_planning.yaml +++ b/ur5_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL diff --git a/ur5e_moveit_config/config/ompl_planning.yaml b/ur5e_moveit_config/config/ompl_planning.yaml index a1d28f721..5026dcdf9 100644 --- a/ur5e_moveit_config/config/ompl_planning.yaml +++ b/ur5e_moveit_config/config/ompl_planning.yaml @@ -121,6 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 manipulator: + longest_valid_segment_fraction: 0.01 default_planner_config: RRTConnect planner_configs: - SBL From 7917d497fa4d116214ff5dc26c680137f9853c59 Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Tue, 28 Sep 2021 20:24:42 +0200 Subject: [PATCH 21/28] moveit: base robots don't have EEFs So don't configure one as part of the MoveIt config. --- .../config/fake_controllers.yaml | 5 +--- ur10_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur10_moveit_config/config/ur10.srdf | 5 ---- .../config/fake_controllers.yaml | 5 +--- ur10e_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur10e_moveit_config/config/ur10e.srdf | 5 ---- .../config/fake_controllers.yaml | 5 +--- ur16e_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur16e_moveit_config/config/ur16e.srdf | 5 ---- .../config/fake_controllers.yaml | 5 +--- ur3_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur3_moveit_config/config/ur3.srdf | 5 ---- .../config/fake_controllers.yaml | 5 +--- ur3e_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur3e_moveit_config/config/ur3e.srdf | 5 ---- .../config/fake_controllers.yaml | 5 +--- ur5_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur5_moveit_config/config/ur5.srdf | 5 ---- .../config/fake_controllers.yaml | 5 +--- ur5e_moveit_config/config/ompl_planning.yaml | 25 ------------------- ur5e_moveit_config/config/ur5e.srdf | 5 ---- 21 files changed, 7 insertions(+), 238 deletions(-) diff --git a/ur10_moveit_config/config/fake_controllers.yaml b/ur10_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur10_moveit_config/config/fake_controllers.yaml +++ b/ur10_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur10_moveit_config/config/ompl_planning.yaml b/ur10_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur10_moveit_config/config/ompl_planning.yaml +++ b/ur10_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur10_moveit_config/config/ur10.srdf b/ur10_moveit_config/config/ur10.srdf index 48ae543d2..0c456855c 100644 --- a/ur10_moveit_config/config/ur10.srdf +++ b/ur10_moveit_config/config/ur10.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - diff --git a/ur10e_moveit_config/config/fake_controllers.yaml b/ur10e_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur10e_moveit_config/config/fake_controllers.yaml +++ b/ur10e_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur10e_moveit_config/config/ompl_planning.yaml b/ur10e_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur10e_moveit_config/config/ompl_planning.yaml +++ b/ur10e_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur10e_moveit_config/config/ur10e.srdf b/ur10e_moveit_config/config/ur10e.srdf index 77bccf0d3..6d3e8f368 100644 --- a/ur10e_moveit_config/config/ur10e.srdf +++ b/ur10e_moveit_config/config/ur10e.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - diff --git a/ur16e_moveit_config/config/fake_controllers.yaml b/ur16e_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur16e_moveit_config/config/fake_controllers.yaml +++ b/ur16e_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur16e_moveit_config/config/ompl_planning.yaml b/ur16e_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur16e_moveit_config/config/ompl_planning.yaml +++ b/ur16e_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur16e_moveit_config/config/ur16e.srdf b/ur16e_moveit_config/config/ur16e.srdf index 5dc0b2e00..bee309614 100644 --- a/ur16e_moveit_config/config/ur16e.srdf +++ b/ur16e_moveit_config/config/ur16e.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - diff --git a/ur3_moveit_config/config/fake_controllers.yaml b/ur3_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur3_moveit_config/config/fake_controllers.yaml +++ b/ur3_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur3_moveit_config/config/ompl_planning.yaml b/ur3_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur3_moveit_config/config/ompl_planning.yaml +++ b/ur3_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur3_moveit_config/config/ur3.srdf b/ur3_moveit_config/config/ur3.srdf index 5447d2f0f..8a5515966 100644 --- a/ur3_moveit_config/config/ur3.srdf +++ b/ur3_moveit_config/config/ur3.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - diff --git a/ur3e_moveit_config/config/fake_controllers.yaml b/ur3e_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur3e_moveit_config/config/fake_controllers.yaml +++ b/ur3e_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur3e_moveit_config/config/ompl_planning.yaml b/ur3e_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur3e_moveit_config/config/ompl_planning.yaml +++ b/ur3e_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur3e_moveit_config/config/ur3e.srdf b/ur3e_moveit_config/config/ur3e.srdf index feb8f539a..173fe8c7f 100644 --- a/ur3e_moveit_config/config/ur3e.srdf +++ b/ur3e_moveit_config/config/ur3e.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - diff --git a/ur5_moveit_config/config/fake_controllers.yaml b/ur5_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur5_moveit_config/config/fake_controllers.yaml +++ b/ur5_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur5_moveit_config/config/ompl_planning.yaml b/ur5_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur5_moveit_config/config/ompl_planning.yaml +++ b/ur5_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur5_moveit_config/config/ur5.srdf b/ur5_moveit_config/config/ur5.srdf index ddf9b0f37..641144203 100644 --- a/ur5_moveit_config/config/ur5.srdf +++ b/ur5_moveit_config/config/ur5.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - diff --git a/ur5e_moveit_config/config/fake_controllers.yaml b/ur5e_moveit_config/config/fake_controllers.yaml index e328e94d4..afbfa6e6c 100644 --- a/ur5e_moveit_config/config/fake_controllers.yaml +++ b/ur5e_moveit_config/config/fake_controllers.yaml @@ -7,9 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] initial: # Define initial robot poses. - group: manipulator - pose: home \ No newline at end of file + pose: home diff --git a/ur5e_moveit_config/config/ompl_planning.yaml b/ur5e_moveit_config/config/ompl_planning.yaml index 5026dcdf9..39ba9f166 100644 --- a/ur5e_moveit_config/config/ompl_planning.yaml +++ b/ur5e_moveit_config/config/ompl_planning.yaml @@ -147,28 +147,3 @@ manipulator: - LazyPRMstar - SPARS - SPARStwo -endeffector: - planner_configs: - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/ur5e_moveit_config/config/ur5e.srdf b/ur5e_moveit_config/config/ur5e.srdf index 76bc9d0b8..daab73c7b 100644 --- a/ur5e_moveit_config/config/ur5e.srdf +++ b/ur5e_moveit_config/config/ur5e.srdf @@ -12,9 +12,6 @@ - - - @@ -32,8 +29,6 @@ - - From 4b04519438b33051e95bda32b7b09c8620ec4390 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Jul 2022 09:20:06 +0000 Subject: [PATCH 22/28] Do not load the description in move_group.launch by default This was actually an upstream bug that it was inserted like that. --- ur10_moveit_config/launch/move_group.launch | 2 +- ur10e_moveit_config/launch/move_group.launch | 2 +- ur16e_moveit_config/launch/move_group.launch | 2 +- ur3_moveit_config/launch/move_group.launch | 2 +- ur3e_moveit_config/launch/move_group.launch | 2 +- ur5_moveit_config/launch/move_group.launch | 2 +- ur5e_moveit_config/launch/move_group.launch | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ur10_moveit_config/launch/move_group.launch b/ur10_moveit_config/launch/move_group.launch index 4b39f0815..681d8f1ec 100644 --- a/ur10_moveit_config/launch/move_group.launch +++ b/ur10_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + diff --git a/ur10e_moveit_config/launch/move_group.launch b/ur10e_moveit_config/launch/move_group.launch index 0dc118e8e..fd433120e 100644 --- a/ur10e_moveit_config/launch/move_group.launch +++ b/ur10e_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + diff --git a/ur16e_moveit_config/launch/move_group.launch b/ur16e_moveit_config/launch/move_group.launch index 9418b0939..8d9ac81f0 100644 --- a/ur16e_moveit_config/launch/move_group.launch +++ b/ur16e_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + diff --git a/ur3_moveit_config/launch/move_group.launch b/ur3_moveit_config/launch/move_group.launch index c0229dbfd..db95cff0e 100644 --- a/ur3_moveit_config/launch/move_group.launch +++ b/ur3_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + diff --git a/ur3e_moveit_config/launch/move_group.launch b/ur3e_moveit_config/launch/move_group.launch index 5c2ab7ade..cb8cbb7de 100644 --- a/ur3e_moveit_config/launch/move_group.launch +++ b/ur3e_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + diff --git a/ur5_moveit_config/launch/move_group.launch b/ur5_moveit_config/launch/move_group.launch index fe73c1d3a..5eb7b0092 100644 --- a/ur5_moveit_config/launch/move_group.launch +++ b/ur5_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + diff --git a/ur5e_moveit_config/launch/move_group.launch b/ur5e_moveit_config/launch/move_group.launch index 764c400bf..e55b731ed 100644 --- a/ur5e_moveit_config/launch/move_group.launch +++ b/ur5e_moveit_config/launch/move_group.launch @@ -37,7 +37,7 @@ " /> --> - + From 9bcb0aa59762606cb636e042887715276a165145 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Jul 2022 09:44:52 +0000 Subject: [PATCH 23/28] Do default to preconfigured RViz config if no config is given --- ur10_moveit_config/launch/demo.launch | 2 +- ur10_moveit_config/launch/moveit_rviz.launch | 6 +++--- ur10e_moveit_config/launch/demo.launch | 2 +- ur10e_moveit_config/launch/moveit_rviz.launch | 6 +++--- ur16e_moveit_config/launch/demo.launch | 2 +- ur16e_moveit_config/launch/moveit_rviz.launch | 6 +++--- ur3_moveit_config/launch/demo.launch | 2 +- ur3_moveit_config/launch/moveit_rviz.launch | 6 +++--- ur3e_moveit_config/launch/demo.launch | 2 +- ur3e_moveit_config/launch/moveit_rviz.launch | 6 +++--- ur5_moveit_config/launch/demo.launch | 2 +- ur5_moveit_config/launch/moveit_rviz.launch | 6 +++--- ur5e_moveit_config/launch/demo.launch | 2 +- ur5e_moveit_config/launch/moveit_rviz.launch | 6 +++--- 14 files changed, 28 insertions(+), 28 deletions(-) diff --git a/ur10_moveit_config/launch/demo.launch b/ur10_moveit_config/launch/demo.launch index 6fe0ed6df..57b3ca5a6 100644 --- a/ur10_moveit_config/launch/demo.launch +++ b/ur10_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur10_moveit_config/launch/moveit_rviz.launch b/ur10_moveit_config/launch/moveit_rviz.launch index a4605c037..53959d703 100644 --- a/ur10_moveit_config/launch/moveit_rviz.launch +++ b/ur10_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + diff --git a/ur10e_moveit_config/launch/demo.launch b/ur10e_moveit_config/launch/demo.launch index 32ba509f0..91a0e581b 100644 --- a/ur10e_moveit_config/launch/demo.launch +++ b/ur10e_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur10e_moveit_config/launch/moveit_rviz.launch b/ur10e_moveit_config/launch/moveit_rviz.launch index a4605c037..285d81657 100644 --- a/ur10e_moveit_config/launch/moveit_rviz.launch +++ b/ur10e_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + diff --git a/ur16e_moveit_config/launch/demo.launch b/ur16e_moveit_config/launch/demo.launch index 6d8e7ab8c..8cc5c85e9 100644 --- a/ur16e_moveit_config/launch/demo.launch +++ b/ur16e_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur16e_moveit_config/launch/moveit_rviz.launch b/ur16e_moveit_config/launch/moveit_rviz.launch index a4605c037..4a06506ce 100644 --- a/ur16e_moveit_config/launch/moveit_rviz.launch +++ b/ur16e_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + diff --git a/ur3_moveit_config/launch/demo.launch b/ur3_moveit_config/launch/demo.launch index 26d9aa40f..87da0471c 100644 --- a/ur3_moveit_config/launch/demo.launch +++ b/ur3_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur3_moveit_config/launch/moveit_rviz.launch b/ur3_moveit_config/launch/moveit_rviz.launch index a4605c037..942ba156e 100644 --- a/ur3_moveit_config/launch/moveit_rviz.launch +++ b/ur3_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + diff --git a/ur3e_moveit_config/launch/demo.launch b/ur3e_moveit_config/launch/demo.launch index 83a323361..b0771f14d 100644 --- a/ur3e_moveit_config/launch/demo.launch +++ b/ur3e_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur3e_moveit_config/launch/moveit_rviz.launch b/ur3e_moveit_config/launch/moveit_rviz.launch index a4605c037..fd1842624 100644 --- a/ur3e_moveit_config/launch/moveit_rviz.launch +++ b/ur3e_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + diff --git a/ur5_moveit_config/launch/demo.launch b/ur5_moveit_config/launch/demo.launch index 0b0a89bcc..865f116f0 100644 --- a/ur5_moveit_config/launch/demo.launch +++ b/ur5_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur5_moveit_config/launch/moveit_rviz.launch b/ur5_moveit_config/launch/moveit_rviz.launch index a4605c037..45ffce7c5 100644 --- a/ur5_moveit_config/launch/moveit_rviz.launch +++ b/ur5_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + diff --git a/ur5e_moveit_config/launch/demo.launch b/ur5e_moveit_config/launch/demo.launch index d00d93522..eef307e03 100644 --- a/ur5e_moveit_config/launch/demo.launch +++ b/ur5e_moveit_config/launch/demo.launch @@ -51,7 +51,7 @@ - + diff --git a/ur5e_moveit_config/launch/moveit_rviz.launch b/ur5e_moveit_config/launch/moveit_rviz.launch index a4605c037..42c17cd43 100644 --- a/ur5e_moveit_config/launch/moveit_rviz.launch +++ b/ur5e_moveit_config/launch/moveit_rviz.launch @@ -4,9 +4,9 @@ - - - + + + From faeb0988f337f51d03d1b0d5a32b0e5231415d5e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Jul 2022 10:01:15 +0000 Subject: [PATCH 24/28] Update moveit package dependencies --- ur10_moveit_config/package.xml | 16 +++++++++++----- ur10e_moveit_config/package.xml | 16 +++++++++++----- ur16e_moveit_config/package.xml | 16 +++++++++++----- ur3_moveit_config/package.xml | 16 +++++++++++----- ur3e_moveit_config/package.xml | 16 +++++++++++----- ur5_moveit_config/package.xml | 16 +++++++++++----- ur5e_moveit_config/package.xml | 16 +++++++++++----- 7 files changed, 77 insertions(+), 35 deletions(-) diff --git a/ur10_moveit_config/package.xml b/ur10_moveit_config/package.xml index 6eab01629..5894a17f9 100644 --- a/ur10_moveit_config/package.xml +++ b/ur10_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin diff --git a/ur10e_moveit_config/package.xml b/ur10e_moveit_config/package.xml index 9f32bbb42..135f4ca1d 100644 --- a/ur10e_moveit_config/package.xml +++ b/ur10e_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin diff --git a/ur16e_moveit_config/package.xml b/ur16e_moveit_config/package.xml index fe5b33521..9fd739e59 100644 --- a/ur16e_moveit_config/package.xml +++ b/ur16e_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin diff --git a/ur3_moveit_config/package.xml b/ur3_moveit_config/package.xml index 73738e439..b8b67688b 100644 --- a/ur3_moveit_config/package.xml +++ b/ur3_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin diff --git a/ur3e_moveit_config/package.xml b/ur3e_moveit_config/package.xml index 36692086f..0c6796446 100644 --- a/ur3e_moveit_config/package.xml +++ b/ur3e_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin diff --git a/ur5_moveit_config/package.xml b/ur5_moveit_config/package.xml index 160034ba8..3b66fc54d 100644 --- a/ur5_moveit_config/package.xml +++ b/ur5_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin diff --git a/ur5e_moveit_config/package.xml b/ur5e_moveit_config/package.xml index 98ab9abd6..23c6c407c 100644 --- a/ur5e_moveit_config/package.xml +++ b/ur5e_moveit_config/package.xml @@ -17,19 +17,25 @@ https://github.com/ros-planning/moveit_setup_assistant/issues https://github.com/ros-planning/moveit_setup_assistant - moveit_ros_move_group + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_move_group moveit_ros_visualization - moveit_fake_controller_manager + moveit_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant moveit_simple_controller_manager - joint_state_publisher robot_state_publisher + rviz tf2_ros + trac_ik_kinematics_plugin + ur_description xacro - ur_description roslaunch catkin - trac_ik_kinematics_plugin From cdb682b0a0be55f797138e891828c180b76fa14e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Jul 2022 12:19:07 +0000 Subject: [PATCH 25/28] Remove unused launch files Those got added before renaming the robot in the srdf --- .../ur10_robot_moveit_controller_manager.launch.xml | 10 ---------- .../launch/ur10_robot_moveit_sensor_manager.launch.xml | 3 --- .../ur10e_robot_moveit_controller_manager.launch.xml | 10 ---------- .../ur10e_robot_moveit_sensor_manager.launch.xml | 3 --- .../ur16e_robot_moveit_controller_manager.launch.xml | 10 ---------- .../ur16e_robot_moveit_sensor_manager.launch.xml | 3 --- .../ur3_robot_moveit_controller_manager.launch.xml | 10 ---------- .../launch/ur3_robot_moveit_sensor_manager.launch.xml | 3 --- .../ur3e_robot_moveit_controller_manager.launch.xml | 10 ---------- .../launch/ur3e_robot_moveit_sensor_manager.launch.xml | 3 --- .../ur5_robot_moveit_controller_manager.launch.xml | 10 ---------- .../launch/ur5_robot_moveit_sensor_manager.launch.xml | 3 --- .../ur5e_robot_moveit_controller_manager.launch.xml | 10 ---------- .../launch/ur5e_robot_moveit_sensor_manager.launch.xml | 3 --- 14 files changed, 91 deletions(-) delete mode 100644 ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml delete mode 100644 ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml delete mode 100644 ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml delete mode 100644 ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml delete mode 100644 ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml delete mode 100644 ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml delete mode 100644 ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml delete mode 100644 ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml delete mode 100644 ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml delete mode 100644 ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml delete mode 100644 ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml delete mode 100644 ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml delete mode 100644 ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml delete mode 100644 ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml diff --git a/ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml b/ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index 252f650c3..000000000 --- a/ur10_moveit_config/launch/ur10_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml b/ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur10_moveit_config/launch/ur10_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml b/ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index b4b5dd22b..000000000 --- a/ur10e_moveit_config/launch/ur10e_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml b/ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur10e_moveit_config/launch/ur10e_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index 0a65168d1..000000000 --- a/ur16e_moveit_config/launch/ur16e_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur16e_moveit_config/launch/ur16e_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml b/ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index eb0bd44b8..000000000 --- a/ur3_moveit_config/launch/ur3_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml b/ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur3_moveit_config/launch/ur3_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index 82dda5ce2..000000000 --- a/ur3e_moveit_config/launch/ur3e_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur3e_moveit_config/launch/ur3e_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml b/ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index 0500324ce..000000000 --- a/ur5_moveit_config/launch/ur5_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml b/ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur5_moveit_config/launch/ur5_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml deleted file mode 100644 index 58e68a4ab..000000000 --- a/ur5e_moveit_config/launch/ur5e_robot_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7..000000000 --- a/ur5e_moveit_config/launch/ur5e_robot_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - From e412bd20495ff0aff48bda02b10c4f4b3c9d38f6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Jul 2022 12:19:31 +0000 Subject: [PATCH 26/28] Rename the planning_execution launchfiles according to ros-i standards --- ...planning_execution.launch => moveit_planning_execution.launch} | 0 ...planning_execution.launch => moveit_planning_execution.launch} | 0 ...planning_execution.launch => moveit_planning_execution.launch} | 0 ...planning_execution.launch => moveit_planning_execution.launch} | 0 ...planning_execution.launch => moveit_planning_execution.launch} | 0 ...planning_execution.launch => moveit_planning_execution.launch} | 0 ...planning_execution.launch => moveit_planning_execution.launch} | 0 7 files changed, 0 insertions(+), 0 deletions(-) rename ur10_moveit_config/launch/{ur10_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) rename ur10e_moveit_config/launch/{ur10e_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) rename ur16e_moveit_config/launch/{ur16e_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) rename ur3_moveit_config/launch/{ur3_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) rename ur3e_moveit_config/launch/{ur3e_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) rename ur5_moveit_config/launch/{ur5_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) rename ur5e_moveit_config/launch/{ur5e_moveit_planning_execution.launch => moveit_planning_execution.launch} (100%) diff --git a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch b/ur10_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur10_moveit_config/launch/ur10_moveit_planning_execution.launch rename to ur10_moveit_config/launch/moveit_planning_execution.launch diff --git a/ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch b/ur10e_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur10e_moveit_config/launch/ur10e_moveit_planning_execution.launch rename to ur10e_moveit_config/launch/moveit_planning_execution.launch diff --git a/ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch b/ur16e_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur16e_moveit_config/launch/ur16e_moveit_planning_execution.launch rename to ur16e_moveit_config/launch/moveit_planning_execution.launch diff --git a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch b/ur3_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur3_moveit_config/launch/ur3_moveit_planning_execution.launch rename to ur3_moveit_config/launch/moveit_planning_execution.launch diff --git a/ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch b/ur3e_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur3e_moveit_config/launch/ur3e_moveit_planning_execution.launch rename to ur3e_moveit_config/launch/moveit_planning_execution.launch diff --git a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch b/ur5_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur5_moveit_config/launch/ur5_moveit_planning_execution.launch rename to ur5_moveit_config/launch/moveit_planning_execution.launch diff --git a/ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch b/ur5e_moveit_config/launch/moveit_planning_execution.launch similarity index 100% rename from ur5e_moveit_config/launch/ur5e_moveit_planning_execution.launch rename to ur5e_moveit_config/launch/moveit_planning_execution.launch From c3fdfefa93a89e49674439ca44120d4ad80556a5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Jul 2022 13:37:10 +0000 Subject: [PATCH 27/28] Update README instructions for updated moveit configs --- README.md | 33 +++++---------------------------- 1 file changed, 5 insertions(+), 28 deletions(-) diff --git a/README.md b/README.md index 08c2feed9..65c8a4fce 100644 --- a/README.md +++ b/README.md @@ -80,22 +80,11 @@ There exist MoveIt! configuration packages for both robots. For setting up the MoveIt! nodes to allow motion planning run: -```roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch``` +```roslaunch ur5_moveit_config moveit_planning_execution.launch``` For starting up RViz with a configuration including the MoveIt! Motion Planning plugin run: -```roslaunch ur5_moveit_config moveit_rviz.launch config:=true``` - - -NOTE: -As MoveIt! seems to have difficulties with finding plans for the UR with full joint limits [-2pi, 2pi], there is a joint_limited version using joint limits restricted to [-pi,pi]. In order to use this joint limited version, simply use the launch file arguments 'limited', i.e.: - -```roslaunch ur_bringup ur5_bringup.launch limited:=true robot_ip:=IP_OF_THE_ROBOT [reverse_port:=REVERSE_PORT]``` - -```roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch limited:=true``` - -```roslaunch ur5_moveit_config moveit_rviz.launch config:=true``` - +```roslaunch ur5_moveit_config moveit_rviz.launch``` ___Usage with Gazebo Simulation___ There are launch files available to bringup a simulated robot - either UR5 or UR10. @@ -105,7 +94,7 @@ Don't forget to source the correct setup shell files and use a new terminal for To bring up the simulated robot in Gazebo, run: -```roslaunch ur_gazebo ur5.launch``` +```roslaunch ur_gazebo ur5_bringup.launch``` ___MoveIt! with a simulated robot___ @@ -113,20 +102,8 @@ Again, you can use MoveIt! to control the simulated robot. For setting up the MoveIt! nodes to allow motion planning run: -```roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true``` +```roslaunch ur5_moveit_config moveit_planning_execution.launch sim:=true``` For starting up RViz with a configuration including the MoveIt! Motion Planning plugin run: -```roslaunch ur5_moveit_config moveit_rviz.launch config:=true``` - - -NOTE: -As MoveIt! seems to have difficulties with finding plans for the UR with full joint limits [-2pi, 2pi], there is a joint_limited version using joint limits restricted to [-pi,pi]. In order to use this joint limited version, simply use the launch file arguments 'limited', i.e.: - -```roslaunch ur_gazebo ur5.launch limited:=true``` - -```roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true limited:=true``` - -```roslaunch ur5_moveit_config moveit_rviz.launch config:=true``` - - +```roslaunch ur5_moveit_config moveit_rviz.launch``` From 9363d9bac8df7bef7d2030ddfac641b71d39124c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 21 Jul 2022 09:56:10 +0200 Subject: [PATCH 28/28] Fix package name in changelog Co-authored-by: RobertWilbrandt --- ur10e_moveit_config/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur10e_moveit_config/CHANGELOG.rst b/ur10e_moveit_config/CHANGELOG.rst index 86fac4e22..ca9917241 100644 --- a/ur10e_moveit_config/CHANGELOG.rst +++ b/ur10e_moveit_config/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ur10_moveit_config +Changelog for package ur10e_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.2.5 (2019-04-05)