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``` diff --git a/universal_robots/package.xml b/universal_robots/package.xml index 5854f0a9b..02be64f71 100644 --- a/universal_robots/package.xml +++ b/universal_robots/package.xml @@ -20,11 +20,12 @@ catkin - ur10_e_moveit_config + ur10e_moveit_config ur10_moveit_config - ur3_e_moveit_config + ur16e_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/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/kinematics.yaml b/ur10_e_moveit_config/config/kinematics.yaml deleted file mode 100644 index 5d492ac1f..000000000 --- a/ur10_e_moveit_config/config/kinematics.yaml +++ /dev/null @@ -1,5 +0,0 @@ -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/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.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/launch/ur10_e_moveit_planning_execution.launch b/ur10_e_moveit_config/launch/ur10_e_moveit_planning_execution.launch deleted file mode 100644 index b701ce754..000000000 --- a/ur10_e_moveit_config/launch/ur10_e_moveit_planning_execution.launch +++ /dev/null @@ -1,14 +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/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..afbfa6e6c 100644 --- a/ur10_moveit_config/config/fake_controllers.yaml +++ b/ur10_moveit_config/config/fake_controllers.yaml @@ -7,6 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] \ No newline at end of file +initial: # Define initial robot poses. + - group: manipulator + pose: home 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/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml index e37d336c3..51e29f53e 100644 --- a/ur10_moveit_config/config/kinematics.yaml +++ b/ur10_moveit_config/config/kinematics.yaml @@ -4,7 +4,7 @@ # 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/config/ompl_planning.yaml b/ur10_moveit_config/config/ompl_planning.yaml index b58b2d655..39ba9f166 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,106 @@ 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 -endeffector: + default_planner_config: RRTConnect 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..bfc4186c4 --- /dev/null +++ b/ur10_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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_moveit_config/config/ur10.srdf b/ur10_moveit_config/config/ur10.srdf index dc3b1b817..0c456855c 100644 --- a/ur10_moveit_config/config/ur10.srdf +++ b/ur10_moveit_config/config/ur10.srdf @@ -3,17 +3,14 @@ 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 --> - + - - - - + @@ -32,17 +29,12 @@ - - - - - - - - + + + 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..57b3ca5a6 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..681d8f1ec 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..cfc2fec04 100644 --- a/ur10_moveit_config/launch/moveit.rviz +++ b/ur10_moveit_config/launch/moveit.rviz @@ -1,31 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 0 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.5175438523292542 + Tree Height: 259 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -35,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 @@ -47,12 +39,17 @@ 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 Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 + 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 @@ -83,12 +80,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -108,10 +108,6 @@ Visualization Manager: 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 @@ -134,19 +130,20 @@ Visualization Manager: Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trail Step Size: 1 + 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 + TextHeight: 0.07999999821186066 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 Query Goal State: true @@ -157,9 +154,9 @@ 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 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -179,12 +176,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -204,10 +204,6 @@ Visualization Manager: 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 @@ -223,14 +219,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -238,53 +236,45 @@ Visualization Manager: 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 + Class: rviz/XYOrbit + Distance: 2.996500015258789 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + 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.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: + Height: 783 + Help: collapsed: false - Time: + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: collapsed: false - Tool Properties: + MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch b/ur10_moveit_config/launch/moveit_planning_execution.launch similarity index 59% rename from ur10_moveit_config/launch/ur10_moveit_planning_execution.launch rename to ur10_moveit_config/launch/moveit_planning_execution.launch index 553e0775c..80f728f0e 100644 --- a/ur10_moveit_config/launch/ur10_moveit_planning_execution.launch +++ b/ur10_moveit_config/launch/moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - - + + - diff --git a/ur10_moveit_config/launch/moveit_rviz.launch b/ur10_moveit_config/launch/moveit_rviz.launch index 8a9f8bd11..53959d703 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/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/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..5894a17f9 100644 --- a/ur10_moveit_config/package.xml +++ b/ur10_moveit_config/package.xml @@ -17,17 +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 - + diff --git a/ur10_moveit_config/tests/moveit_planning_execution.xml b/ur10_moveit_config/tests/moveit_planning_execution.xml index 57cf9ff80..843f2bece 100644 --- a/ur10_moveit_config/tests/moveit_planning_execution.xml +++ b/ur10_moveit_config/tests/moveit_planning_execution.xml @@ -2,14 +2,13 @@ - - + - - + + 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..ca9917241 --- /dev/null +++ b/ur10e_moveit_config/CHANGELOG.rst @@ -0,0 +1,63 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ur10e_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/ur3_e_moveit_config/CMakeLists.txt b/ur10e_moveit_config/CMakeLists.txt similarity index 92% rename from ur3_e_moveit_config/CMakeLists.txt rename to ur10e_moveit_config/CMakeLists.txt index 13e9deb9d..d665149d3 100644 --- a/ur3_e_moveit_config/CMakeLists.txt +++ b/ur10e_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ur3_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 72% rename from ur10_e_moveit_config/config/fake_controllers.yaml rename to ur10e_moveit_config/config/fake_controllers.yaml index 5e24f9451..afbfa6e6c 100644 --- a/ur10_e_moveit_config/config/fake_controllers.yaml +++ b/ur10e_moveit_config/config/fake_controllers.yaml @@ -7,6 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] \ No newline at end of file +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/ur3_e_moveit_config/config/joint_limits.yaml b/ur10e_moveit_config/config/joint_limits.yaml similarity index 51% rename from ur3_e_moveit_config/config/joint_limits.yaml rename to ur10e_moveit_config/config/joint_limits.yaml index 991719a66..633b7bbce 100644 --- a/ur3_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/ur10e_moveit_config/config/kinematics.yaml b/ur10e_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..51e29f53e --- /dev/null +++ b/ur10e_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: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 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..39ba9f166 --- /dev/null +++ b/ur10e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,149 @@ +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: + longest_valid_segment_fraction: 0.01 + default_planner_config: RRTConnect + 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..bfc4186c4 --- /dev/null +++ b/ur10e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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 80% rename from ur10_e_moveit_config/config/ur10e.srdf rename to ur10e_moveit_config/config/ur10e.srdf index ad686256f..6d3e8f368 100644 --- a/ur10_e_moveit_config/config/ur10e.srdf +++ b/ur10e_moveit_config/config/ur10e.srdf @@ -3,17 +3,14 @@ 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 --> - + - - - - + @@ -32,17 +29,12 @@ - - - - - - - - + + + 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..91a0e581b --- /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/ur3_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur10e_moveit_config/launch/fake_moveit_controller_manager.launch.xml similarity index 82% rename from ur3_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml rename to ur10e_moveit_config/launch/fake_moveit_controller_manager.launch.xml index 630203a59..ed8eb47d5 100644 --- a/ur3_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/ur3_e_moveit_config/launch/move_group.launch b/ur10e_moveit_config/launch/move_group.launch similarity index 61% rename from ur3_e_moveit_config/launch/move_group.launch rename to ur10e_moveit_config/launch/move_group.launch index de6d39dd3..fd433120e 100644 --- a/ur3_e_moveit_config/launch/move_group.launch +++ b/ur10e_moveit_config/launch/move_group.launch @@ -1,15 +1,10 @@ - - - - - + value="gdb -x $(find ur10e_moveit_config)/launch/gdb_settings.gdb --ex run --args" /> @@ -17,27 +12,52 @@ + + + + + + + + + + + + + + + - - + + - + - + - - + + @@ -48,19 +68,9 @@ + + - - diff --git a/ur10_e_moveit_config/launch/moveit.rviz b/ur10e_moveit_config/launch/moveit.rviz similarity index 65% rename from ur10_e_moveit_config/launch/moveit.rviz rename to ur10e_moveit_config/launch/moveit.rviz index c7540069f..cfc2fec04 100644 --- a/ur10_e_moveit_config/launch/moveit.rviz +++ b/ur10e_moveit_config/launch/moveit.rviz @@ -1,31 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 0 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.5175438523292542 + Tree Height: 259 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -35,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 @@ -47,12 +39,17 @@ 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 Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 + 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 @@ -83,12 +80,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -108,10 +108,6 @@ Visualization Manager: 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 @@ -134,19 +130,20 @@ Visualization Manager: Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trail Step Size: 1 + 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 + TextHeight: 0.07999999821186066 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 Query Goal State: true @@ -157,9 +154,9 @@ 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 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -179,12 +176,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -204,10 +204,6 @@ Visualization Manager: 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 @@ -223,14 +219,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -238,53 +236,45 @@ Visualization Manager: 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 + Class: rviz/XYOrbit + Distance: 2.996500015258789 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + 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.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: + Height: 783 + Help: collapsed: false - Time: + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: collapsed: false - Tool Properties: + MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur10e_moveit_config/launch/moveit_planning_execution.launch b/ur10e_moveit_config/launch/moveit_planning_execution.launch new file mode 100644 index 000000000..de9c149f5 --- /dev/null +++ b/ur10e_moveit_config/launch/moveit_planning_execution.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/ur10e_moveit_config/launch/moveit_rviz.launch b/ur10e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..285d81657 --- /dev/null +++ b/ur10e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur3_e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur10e_moveit_config/launch/ompl_planning_pipeline.launch.xml similarity index 92% rename from ur3_e_moveit_config/launch/ompl_planning_pipeline.launch.xml rename to ur10e_moveit_config/launch/ompl_planning_pipeline.launch.xml index 71475878e..a52a7c101 100644 --- a/ur3_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/ur3_e_moveit_config/launch/planning_context.launch b/ur10e_moveit_config/launch/planning_context.launch similarity index 58% rename from ur3_e_moveit_config/launch/planning_context.launch rename to ur10e_moveit_config/launch/planning_context.launch index d1a1618ac..a0121a8de 100644 --- a/ur3_e_moveit_config/launch/planning_context.launch +++ b/ur10e_moveit_config/launch/planning_context.launch @@ -1,28 +1,25 @@ - - - - - + - - + + - + - + + - + diff --git a/ur3_e_moveit_config/launch/planning_pipeline.launch.xml b/ur10e_moveit_config/launch/planning_pipeline.launch.xml similarity index 68% rename from ur3_e_moveit_config/launch/planning_pipeline.launch.xml rename to ur10e_moveit_config/launch/planning_pipeline.launch.xml index 40f9e8d96..277b80706 100644 --- a/ur3_e_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur10e_moveit_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/ur5_e_moveit_config/launch/run_benchmark_ompl.launch b/ur10e_moveit_config/launch/run_benchmark_ompl.launch similarity index 71% rename from ur5_e_moveit_config/launch/run_benchmark_ompl.launch rename to ur10e_moveit_config/launch/run_benchmark_ompl.launch index d37513d82..3553a913f 100644 --- a/ur5_e_moveit_config/launch/run_benchmark_ompl.launch +++ b/ur10e_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/ur3_e_moveit_config/launch/sensor_manager.launch.xml b/ur10e_moveit_config/launch/sensor_manager.launch.xml similarity index 81% rename from ur3_e_moveit_config/launch/sensor_manager.launch.xml rename to ur10e_moveit_config/launch/sensor_manager.launch.xml index 8f17c237b..7140a9f3f 100644 --- a/ur3_e_moveit_config/launch/sensor_manager.launch.xml +++ b/ur10e_moveit_config/launch/sensor_manager.launch.xml @@ -8,7 +8,7 @@ - - + + diff --git a/ur3_e_moveit_config/launch/setup_assistant.launch b/ur10e_moveit_config/launch/setup_assistant.launch similarity index 60% rename from ur3_e_moveit_config/launch/setup_assistant.launch rename to ur10e_moveit_config/launch/setup_assistant.launch index ca7e28b0f..6fc48197b 100644 --- a/ur3_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/ur3_e_moveit_config/launch/trajectory_execution.launch.xml b/ur10e_moveit_config/launch/trajectory_execution.launch.xml similarity index 86% rename from ur3_e_moveit_config/launch/trajectory_execution.launch.xml rename to ur10e_moveit_config/launch/trajectory_execution.launch.xml index 6786cca12..3c25c0689 100644 --- a/ur3_e_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur10e_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/ur10e_moveit_config/launch/ur10e_moveit_controller_manager.launch.xml similarity index 77% rename from ur3_e_moveit_config/launch/ur3_e_moveit_controller_manager.launch.xml rename to ur10e_moveit_config/launch/ur10e_moveit_controller_manager.launch.xml index f54839f4e..114ae0a28 100644 --- a/ur3_e_moveit_config/launch/ur3_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_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/ur3_e_moveit_config/launch/warehouse.launch b/ur10e_moveit_config/launch/warehouse.launch similarity index 71% rename from ur3_e_moveit_config/launch/warehouse.launch rename to ur10e_moveit_config/launch/warehouse.launch index 918a58717..c6548b5d4 100644 --- a/ur3_e_moveit_config/launch/warehouse.launch +++ b/ur10e_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - - + + - + diff --git a/ur3_e_moveit_config/launch/warehouse_settings.launch.xml b/ur10e_moveit_config/launch/warehouse_settings.launch.xml similarity index 64% rename from ur3_e_moveit_config/launch/warehouse_settings.launch.xml rename to ur10e_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur3_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 73% rename from ur10_e_moveit_config/package.xml rename to ur10e_moveit_config/package.xml index bceb8039d..135f4ca1d 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,22 +10,30 @@ 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 + 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 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..fe138e5f1 --- /dev/null +++ b/ur10e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + 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/ur3_e_moveit_config/config/fake_controllers.yaml b/ur16e_moveit_config/config/fake_controllers.yaml similarity index 72% rename from ur3_e_moveit_config/config/fake_controllers.yaml rename to ur16e_moveit_config/config/fake_controllers.yaml index 5e24f9451..afbfa6e6c 100644 --- a/ur3_e_moveit_config/config/fake_controllers.yaml +++ b/ur16e_moveit_config/config/fake_controllers.yaml @@ -7,6 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] \ No newline at end of file +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/ur5_e_moveit_config/config/joint_limits.yaml b/ur16e_moveit_config/config/joint_limits.yaml similarity index 51% rename from ur5_e_moveit_config/config/joint_limits.yaml rename to ur16e_moveit_config/config/joint_limits.yaml index 991719a66..633b7bbce 100644 --- a/ur5_e_moveit_config/config/joint_limits.yaml +++ b/ur16e_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/ur16e_moveit_config/config/kinematics.yaml b/ur16e_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..51e29f53e --- /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: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 diff --git a/ur16e_moveit_config/config/ompl_planning.yaml b/ur16e_moveit_config/config/ompl_planning.yaml new file mode 100644 index 000000000..39ba9f166 --- /dev/null +++ b/ur16e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,149 @@ +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: + longest_valid_segment_fraction: 0.01 + default_planner_config: RRTConnect + 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..bfc4186c4 --- /dev/null +++ b/ur16e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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..bee309614 --- /dev/null +++ b/ur16e_moveit_config/config/ur16e.srdf @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..8cc5c85e9 --- /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/ur5_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur16e_moveit_config/launch/fake_moveit_controller_manager.launch.xml similarity index 82% rename from ur5_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml rename to ur16e_moveit_config/launch/fake_moveit_controller_manager.launch.xml index 8a8e020d7..031184d30 100644 --- a/ur5_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ b/ur16e_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/ur16e_moveit_config/launch/move_group.launch similarity index 60% rename from ur5_e_moveit_config/launch/move_group.launch rename to ur16e_moveit_config/launch/move_group.launch index d39c94716..8d9ac81f0 100644 --- a/ur5_e_moveit_config/launch/move_group.launch +++ b/ur16e_moveit_config/launch/move_group.launch @@ -1,14 +1,10 @@ - - - - - - + @@ -16,27 +12,52 @@ + + + + + + + + + + + + + + + - - + + - + - + - - + + @@ -47,19 +68,9 @@ + + - - diff --git a/ur3_e_moveit_config/launch/moveit.rviz b/ur16e_moveit_config/launch/moveit.rviz similarity index 65% rename from ur3_e_moveit_config/launch/moveit.rviz rename to ur16e_moveit_config/launch/moveit.rviz index c7540069f..cfc2fec04 100644 --- a/ur3_e_moveit_config/launch/moveit.rviz +++ b/ur16e_moveit_config/launch/moveit.rviz @@ -1,31 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 0 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.5175438523292542 + Tree Height: 259 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -35,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 @@ -47,12 +39,17 @@ 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 Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 + 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 @@ -83,12 +80,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -108,10 +108,6 @@ Visualization Manager: 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 @@ -134,19 +130,20 @@ Visualization Manager: Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trail Step Size: 1 + 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 + TextHeight: 0.07999999821186066 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 Query Goal State: true @@ -157,9 +154,9 @@ 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 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -179,12 +176,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -204,10 +204,6 @@ Visualization Manager: 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 @@ -223,14 +219,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -238,53 +236,45 @@ Visualization Manager: 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 + Class: rviz/XYOrbit + Distance: 2.996500015258789 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + 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.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: + Height: 783 + Help: collapsed: false - Time: + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: collapsed: false - Tool Properties: + MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur16e_moveit_config/launch/moveit_planning_execution.launch b/ur16e_moveit_config/launch/moveit_planning_execution.launch new file mode 100644 index 000000000..e3c2882f0 --- /dev/null +++ b/ur16e_moveit_config/launch/moveit_planning_execution.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/ur16e_moveit_config/launch/moveit_rviz.launch b/ur16e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..4a06506ce --- /dev/null +++ b/ur16e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur5_e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml similarity index 92% rename from ur5_e_moveit_config/launch/ompl_planning_pipeline.launch.xml rename to ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml index eb0ef8926..0ac9828c4 100644 --- a/ur5_e_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/ur16e_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -3,7 +3,7 @@ - - + diff --git a/ur5_e_moveit_config/launch/planning_context.launch b/ur16e_moveit_config/launch/planning_context.launch similarity index 58% rename from ur5_e_moveit_config/launch/planning_context.launch rename to ur16e_moveit_config/launch/planning_context.launch index 63109be0a..2c61f888c 100644 --- a/ur5_e_moveit_config/launch/planning_context.launch +++ b/ur16e_moveit_config/launch/planning_context.launch @@ -1,28 +1,25 @@ - - - - - + - - + + - + - + + - + diff --git a/ur5_e_moveit_config/launch/planning_pipeline.launch.xml b/ur16e_moveit_config/launch/planning_pipeline.launch.xml similarity index 68% rename from ur5_e_moveit_config/launch/planning_pipeline.launch.xml rename to ur16e_moveit_config/launch/planning_pipeline.launch.xml index 12211f9d8..fc6e0f94c 100644 --- a/ur5_e_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur16e_moveit_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/ur3_e_moveit_config/launch/run_benchmark_ompl.launch b/ur16e_moveit_config/launch/run_benchmark_ompl.launch similarity index 71% rename from ur3_e_moveit_config/launch/run_benchmark_ompl.launch rename to ur16e_moveit_config/launch/run_benchmark_ompl.launch index e84b5312f..40e7444e6 100644 --- a/ur3_e_moveit_config/launch/run_benchmark_ompl.launch +++ b/ur16e_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/ur5_e_moveit_config/launch/sensor_manager.launch.xml b/ur16e_moveit_config/launch/sensor_manager.launch.xml similarity index 81% rename from ur5_e_moveit_config/launch/sensor_manager.launch.xml rename to ur16e_moveit_config/launch/sensor_manager.launch.xml index 172eb87be..a8db6fdb8 100644 --- a/ur5_e_moveit_config/launch/sensor_manager.launch.xml +++ b/ur16e_moveit_config/launch/sensor_manager.launch.xml @@ -8,7 +8,7 @@ - - + + 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/ur5_e_moveit_config/launch/trajectory_execution.launch.xml b/ur16e_moveit_config/launch/trajectory_execution.launch.xml similarity index 86% rename from ur5_e_moveit_config/launch/trajectory_execution.launch.xml rename to ur16e_moveit_config/launch/trajectory_execution.launch.xml index 3513753a6..099f3c9fe 100644 --- a/ur5_e_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur16e_moveit_config/launch/trajectory_execution.launch.xml @@ -1,6 +1,6 @@ - + @@ -10,9 +10,9 @@ - + - - - + + + 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/ur3_e_moveit_config/launch/ur3_e_moveit_sensor_manager.launch.xml b/ur16e_moveit_config/launch/ur16e_moveit_sensor_manager.launch.xml similarity index 100% rename from ur3_e_moveit_config/launch/ur3_e_moveit_sensor_manager.launch.xml rename to ur16e_moveit_config/launch/ur16e_moveit_sensor_manager.launch.xml diff --git a/ur5_e_moveit_config/launch/warehouse.launch b/ur16e_moveit_config/launch/warehouse.launch similarity index 71% rename from ur5_e_moveit_config/launch/warehouse.launch rename to ur16e_moveit_config/launch/warehouse.launch index 3ac1ddf6a..529d5e2a6 100644 --- a/ur5_e_moveit_config/launch/warehouse.launch +++ b/ur16e_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - - + + - + diff --git a/ur10_e_moveit_config/launch/warehouse_settings.launch.xml b/ur16e_moveit_config/launch/warehouse_settings.launch.xml similarity index 64% rename from ur10_e_moveit_config/launch/warehouse_settings.launch.xml rename to ur16e_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur10_e_moveit_config/launch/warehouse_settings.launch.xml +++ b/ur16e_moveit_config/launch/warehouse_settings.launch.xml @@ -1,15 +1,16 @@ - - + + - - + + + diff --git a/ur16e_moveit_config/package.xml b/ur16e_moveit_config/package.xml new file mode 100644 index 000000000..9fd739e59 --- /dev/null +++ b/ur16e_moveit_config/package.xml @@ -0,0 +1,41 @@ + + + + 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 + + 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_ros_warehouse + warehouse_ros_mongo + moveit_setup_assistant + moveit_simple_controller_manager + robot_state_publisher + rviz + tf2_ros + trac_ik_kinematics_plugin + ur_description + xacro + + 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..3ff79057b --- /dev/null +++ b/ur16e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + 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/kinematics.yaml b/ur3_e_moveit_config/config/kinematics.yaml deleted file mode 100644 index 5d492ac1f..000000000 --- a/ur3_e_moveit_config/config/kinematics.yaml +++ /dev/null @@ -1,5 +0,0 @@ -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/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.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/launch/ur3_e_moveit_planning_execution.launch b/ur3_e_moveit_config/launch/ur3_e_moveit_planning_execution.launch deleted file mode 100644 index 75b83fee8..000000000 --- a/ur3_e_moveit_config/launch/ur3_e_moveit_planning_execution.launch +++ /dev/null @@ -1,14 +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/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..afbfa6e6c 100644 --- a/ur3_moveit_config/config/fake_controllers.yaml +++ b/ur3_moveit_config/config/fake_controllers.yaml @@ -7,6 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] \ No newline at end of file +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/ur3_moveit_config/config/joint_limits.yaml b/ur3_moveit_config/config/joint_limits.yaml index 60797d513..45694aa56 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: 3.1415926535897931 + 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: 3.1415926535897931 + 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: 6.283185307179586 + 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: 6.283185307179586 + 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: 6.283185307179586 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/ur3_moveit_config/config/kinematics.yaml b/ur3_moveit_config/config/kinematics.yaml index 7162d768c..51e29f53e 100644 --- a/ur3_moveit_config/config/kinematics.yaml +++ b/ur3_moveit_config/config/kinematics.yaml @@ -1,10 +1,10 @@ #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 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/config/ompl_planning.yaml b/ur3_moveit_config/config/ompl_planning.yaml index b58b2d655..39ba9f166 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,106 @@ 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 -endeffector: + default_planner_config: RRTConnect 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..bfc4186c4 --- /dev/null +++ b/ur3_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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..8a5515966 100644 --- a/ur3_moveit_config/config/ur3.srdf +++ b/ur3_moveit_config/config/ur3.srdf @@ -3,17 +3,14 @@ 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 --> - + - - - - + @@ -32,18 +29,14 @@ - - - - - - + + + - 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..87da0471c 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..db95cff0e 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..cfc2fec04 100644 --- a/ur3_moveit_config/launch/moveit.rviz +++ b/ur3_moveit_config/launch/moveit.rviz @@ -1,31 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 0 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.5175438523292542 + Tree Height: 259 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -35,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 @@ -47,12 +39,17 @@ 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 Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 + 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 @@ -83,12 +80,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -108,10 +108,6 @@ Visualization Manager: 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 @@ -134,19 +130,20 @@ Visualization Manager: Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trail Step Size: 1 + 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 + TextHeight: 0.07999999821186066 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 Query Goal State: true @@ -157,9 +154,9 @@ 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 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -179,12 +176,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -204,10 +204,6 @@ Visualization Manager: 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 @@ -223,14 +219,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -238,53 +236,45 @@ Visualization Manager: 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 + Class: rviz/XYOrbit + Distance: 2.996500015258789 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + 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.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: + Height: 783 + Help: collapsed: false - Time: + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: collapsed: false - Tool Properties: + MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch b/ur3_moveit_config/launch/moveit_planning_execution.launch similarity index 58% rename from ur3_moveit_config/launch/ur3_moveit_planning_execution.launch rename to ur3_moveit_config/launch/moveit_planning_execution.launch index 81b7ca8a7..3c97229dd 100644 --- a/ur3_moveit_config/launch/ur3_moveit_planning_execution.launch +++ b/ur3_moveit_config/launch/moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - - + + - diff --git a/ur3_moveit_config/launch/moveit_rviz.launch b/ur3_moveit_config/launch/moveit_rviz.launch index 8aa1b22d5..942ba156e 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/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/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..b8b67688b 100644 --- a/ur3_moveit_config/package.xml +++ b/ur3_moveit_config/package.xml @@ -17,17 +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 - + diff --git a/ur3_moveit_config/tests/moveit_planning_execution.xml b/ur3_moveit_config/tests/moveit_planning_execution.xml index ca37b19ed..10246c7d7 100644 --- a/ur3_moveit_config/tests/moveit_planning_execution.xml +++ b/ur3_moveit_config/tests/moveit_planning_execution.xml @@ -1,16 +1,14 @@ - - - + - - + + 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/ur5_e_moveit_config/CMakeLists.txt b/ur3e_moveit_config/CMakeLists.txt similarity index 92% rename from ur5_e_moveit_config/CMakeLists.txt rename to ur3e_moveit_config/CMakeLists.txt index 8e4d03bd5..73325ab56 100644 --- a/ur5_e_moveit_config/CMakeLists.txt +++ b/ur3e_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ur5_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/ur5_e_moveit_config/config/fake_controllers.yaml b/ur3e_moveit_config/config/fake_controllers.yaml similarity index 72% rename from ur5_e_moveit_config/config/fake_controllers.yaml rename to ur3e_moveit_config/config/fake_controllers.yaml index 5e24f9451..afbfa6e6c 100644 --- a/ur5_e_moveit_config/config/fake_controllers.yaml +++ b/ur3e_moveit_config/config/fake_controllers.yaml @@ -7,6 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] \ No newline at end of file +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/ur10_e_moveit_config/config/joint_limits.yaml b/ur3e_moveit_config/config/joint_limits.yaml similarity index 52% rename from ur10_e_moveit_config/config/joint_limits.yaml rename to ur3e_moveit_config/config/joint_limits.yaml index 991719a66..45694aa56 100644 --- a/ur10_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: 3.1415926535897931 + 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: 3.1415926535897931 + 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: 6.283185307179586 + 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: 6.283185307179586 + 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: 6.283185307179586 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/ur3e_moveit_config/config/kinematics.yaml b/ur3e_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..51e29f53e --- /dev/null +++ b/ur3e_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: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 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..39ba9f166 --- /dev/null +++ b/ur3e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,149 @@ +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: + longest_valid_segment_fraction: 0.01 + default_planner_config: RRTConnect + 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..bfc4186c4 --- /dev/null +++ b/ur3e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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 81% rename from ur3_e_moveit_config/config/ur3e.srdf rename to ur3e_moveit_config/config/ur3e.srdf index b47172108..173fe8c7f 100644 --- a/ur3_e_moveit_config/config/ur3e.srdf +++ b/ur3e_moveit_config/config/ur3e.srdf @@ -3,17 +3,14 @@ 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 --> - + - - - - + @@ -32,18 +29,14 @@ - - - - - - + + + - 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..b0771f14d --- /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/ur10_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 ur10_e_moveit_config/launch/fake_moveit_controller_manager.launch.xml rename to ur3e_moveit_config/launch/fake_moveit_controller_manager.launch.xml index 1f3f4de92..8789fd70c 100644 --- a/ur10_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/ur10_e_moveit_config/launch/move_group.launch b/ur3e_moveit_config/launch/move_group.launch similarity index 54% rename from ur10_e_moveit_config/launch/move_group.launch rename to ur3e_moveit_config/launch/move_group.launch index efeb0a045..cb8cbb7de 100644 --- a/ur10_e_moveit_config/launch/move_group.launch +++ b/ur3e_moveit_config/launch/move_group.launch @@ -1,14 +1,10 @@ - - - - - - + @@ -16,27 +12,52 @@ + + + + + + + + + + + + + + + - - + + - + - + - - + + @@ -47,19 +68,9 @@ + + - - diff --git a/ur5_e_moveit_config/launch/moveit.rviz b/ur3e_moveit_config/launch/moveit.rviz similarity index 65% rename from ur5_e_moveit_config/launch/moveit.rviz rename to ur3e_moveit_config/launch/moveit.rviz index c7540069f..cfc2fec04 100644 --- a/ur5_e_moveit_config/launch/moveit.rviz +++ b/ur3e_moveit_config/launch/moveit.rviz @@ -1,31 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 0 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.5175438523292542 + Tree Height: 259 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -35,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 @@ -47,12 +39,17 @@ 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 Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 + 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 @@ -83,12 +80,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -108,10 +108,6 @@ Visualization Manager: 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 @@ -134,19 +130,20 @@ Visualization Manager: Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trail Step Size: 1 + 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 + TextHeight: 0.07999999821186066 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 Query Goal State: true @@ -157,9 +154,9 @@ 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 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -179,12 +176,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -204,10 +204,6 @@ Visualization Manager: 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 @@ -223,14 +219,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -238,53 +236,45 @@ Visualization Manager: 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 + Class: rviz/XYOrbit + Distance: 2.996500015258789 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + 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.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: + Height: 783 + Help: collapsed: false - Time: + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: collapsed: false - Tool Properties: + MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur3e_moveit_config/launch/moveit_planning_execution.launch b/ur3e_moveit_config/launch/moveit_planning_execution.launch new file mode 100644 index 000000000..939c0769b --- /dev/null +++ b/ur3e_moveit_config/launch/moveit_planning_execution.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/ur3e_moveit_config/launch/moveit_rviz.launch b/ur3e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..fd1842624 --- /dev/null +++ b/ur3e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur10_e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur3e_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 ur3e_moveit_config/launch/ompl_planning_pipeline.launch.xml index 2a6b4c1a7..bf3b34698 100644 --- a/ur10_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/ur10_e_moveit_config/launch/planning_context.launch b/ur3e_moveit_config/launch/planning_context.launch similarity index 54% rename from ur10_e_moveit_config/launch/planning_context.launch rename to ur3e_moveit_config/launch/planning_context.launch index a87b942d1..49ef7dfeb 100644 --- a/ur10_e_moveit_config/launch/planning_context.launch +++ b/ur3e_moveit_config/launch/planning_context.launch @@ -1,28 +1,25 @@ - - - - - + - - + + - + - + + - + diff --git a/ur10_e_moveit_config/launch/planning_pipeline.launch.xml b/ur3e_moveit_config/launch/planning_pipeline.launch.xml similarity index 57% rename from ur10_e_moveit_config/launch/planning_pipeline.launch.xml rename to ur3e_moveit_config/launch/planning_pipeline.launch.xml index a0d6c2960..5dfab7d5a 100644 --- a/ur10_e_moveit_config/launch/planning_pipeline.launch.xml +++ b/ur3e_moveit_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/ur10_e_moveit_config/launch/run_benchmark_ompl.launch b/ur3e_moveit_config/launch/run_benchmark_ompl.launch similarity index 63% rename from ur10_e_moveit_config/launch/run_benchmark_ompl.launch rename to ur3e_moveit_config/launch/run_benchmark_ompl.launch index 9c4615c8d..46f8cb9f3 100644 --- a/ur10_e_moveit_config/launch/run_benchmark_ompl.launch +++ b/ur3e_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/ur10_e_moveit_config/launch/sensor_manager.launch.xml b/ur3e_moveit_config/launch/sensor_manager.launch.xml similarity index 70% rename from ur10_e_moveit_config/launch/sensor_manager.launch.xml rename to ur3e_moveit_config/launch/sensor_manager.launch.xml index 90cf60d85..c6a1afaae 100644 --- a/ur10_e_moveit_config/launch/sensor_manager.launch.xml +++ b/ur3e_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/ur3e_moveit_config/launch/setup_assistant.launch similarity index 60% rename from ur10_e_moveit_config/launch/setup_assistant.launch rename to ur3e_moveit_config/launch/setup_assistant.launch index f35f15b87..215c93dce 100644 --- a/ur10_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/ur10_e_moveit_config/launch/trajectory_execution.launch.xml b/ur3e_moveit_config/launch/trajectory_execution.launch.xml similarity index 83% rename from ur10_e_moveit_config/launch/trajectory_execution.launch.xml rename to ur3e_moveit_config/launch/trajectory_execution.launch.xml index 2dfb6e8bf..aadaeb14c 100644 --- a/ur10_e_moveit_config/launch/trajectory_execution.launch.xml +++ b/ur3e_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/ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml similarity index 78% rename from ur10_e_moveit_config/launch/ur10_e_moveit_controller_manager.launch.xml rename to ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml index ac361c10a..ec00c74b6 100644 --- a/ur10_e_moveit_config/launch/ur10_e_moveit_controller_manager.launch.xml +++ b/ur3e_moveit_config/launch/ur3e_moveit_controller_manager.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/ur5_e_moveit_config/launch/ur5_e_moveit_sensor_manager.launch.xml b/ur3e_moveit_config/launch/ur3e_moveit_sensor_manager.launch.xml similarity index 100% rename from ur5_e_moveit_config/launch/ur5_e_moveit_sensor_manager.launch.xml rename to ur3e_moveit_config/launch/ur3e_moveit_sensor_manager.launch.xml diff --git a/ur10_e_moveit_config/launch/warehouse.launch b/ur3e_moveit_config/launch/warehouse.launch similarity index 67% rename from ur10_e_moveit_config/launch/warehouse.launch rename to ur3e_moveit_config/launch/warehouse.launch index b0c8eeefe..8916787cb 100644 --- a/ur10_e_moveit_config/launch/warehouse.launch +++ b/ur3e_moveit_config/launch/warehouse.launch @@ -1,13 +1,13 @@ - + - - + + - + diff --git a/ur5_e_moveit_config/launch/warehouse_settings.launch.xml b/ur3e_moveit_config/launch/warehouse_settings.launch.xml similarity index 64% rename from ur5_e_moveit_config/launch/warehouse_settings.launch.xml rename to ur3e_moveit_config/launch/warehouse_settings.launch.xml index d11aaeb21..e473b083b 100644 --- a/ur5_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 73% rename from ur3_e_moveit_config/package.xml rename to ur3e_moveit_config/package.xml index bfdb3f9bc..0c6796446 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,22 +10,30 @@ 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 + 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 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..6025d2427 --- /dev/null +++ b/ur3e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + 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/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 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/kinematics.yaml b/ur5_e_moveit_config/config/kinematics.yaml deleted file mode 100644 index 5d492ac1f..000000000 --- a/ur5_e_moveit_config/config/kinematics.yaml +++ /dev/null @@ -1,5 +0,0 @@ -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/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.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/launch/ur5_e_moveit_planning_execution.launch b/ur5_e_moveit_config/launch/ur5_e_moveit_planning_execution.launch deleted file mode 100644 index 148c40e4b..000000000 --- a/ur5_e_moveit_config/launch/ur5_e_moveit_planning_execution.launch +++ /dev/null @@ -1,14 +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/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..afbfa6e6c 100644 --- a/ur5_moveit_config/config/fake_controllers.yaml +++ b/ur5_moveit_config/config/fake_controllers.yaml @@ -7,6 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint - - name: fake_endeffector_controller - joints: - [] \ No newline at end of file +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/ur5_moveit_config/config/joint_limits.yaml b/ur5_moveit_config/config/joint_limits.yaml index 60797d513..bdc914f89 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: 3.1415926535897931 + 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: 3.1415926535897931 + 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 diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml index e2fa9b157..51e29f53e 100644 --- a/ur5_moveit_config/config/kinematics.yaml +++ b/ur5_moveit_config/config/kinematics.yaml @@ -1,10 +1,10 @@ #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 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/config/ompl_planning.yaml b/ur5_moveit_config/config/ompl_planning.yaml index b58b2d655..39ba9f166 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,106 @@ 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 -endeffector: + default_planner_config: RRTConnect 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..bfc4186c4 --- /dev/null +++ b/ur5_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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..641144203 100644 --- a/ur5_moveit_config/config/ur5.srdf +++ b/ur5_moveit_config/config/ur5.srdf @@ -3,17 +3,14 @@ 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 --> - + - - - - + @@ -32,18 +29,17 @@ - - - - - - + + + + + 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..865f116f0 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..5eb7b0092 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..cfc2fec04 100644 --- a/ur5_moveit_config/launch/moveit.rviz +++ b/ur5_moveit_config/launch/moveit.rviz @@ -1,31 +1,23 @@ Panels: - Class: rviz/Displays - Help Height: 75 + Help Height: 0 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.5175438523292542 + Tree Height: 259 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -35,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 @@ -47,12 +39,17 @@ 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 Move Group Namespace: "" - MoveIt_Goal_Tolerance: 0 + 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 @@ -83,12 +80,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -108,10 +108,6 @@ Visualization Manager: 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 @@ -134,19 +130,20 @@ Visualization Manager: Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trajectory Topic: /move_group/display_planned_path + Trail Step Size: 1 + 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 + TextHeight: 0.07999999821186066 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 Query Goal State: true @@ -157,9 +154,9 @@ 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 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -179,12 +176,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true - ee_link: + base_link_inertia: Alpha: 1 Show Axes: false Show Trail: false Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false forearm_link: Alpha: 1 Show Axes: false @@ -204,10 +204,6 @@ Visualization Manager: 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 @@ -223,14 +219,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 1 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Default Light: true + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -238,53 +236,45 @@ Visualization Manager: 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 + Class: rviz/XYOrbit + Distance: 2.996500015258789 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.044143 - Y: -0.0136969 - Z: 0.0199439 + 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.465398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.9954 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 997 - Hide Left Dock: false - Hide Right Dock: true - Motion Planning: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000027e0000038afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb000001c000ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000040c0000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: + Height: 783 + Help: collapsed: false - Time: + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: collapsed: false - Tool Properties: + MotionPlanning - Trajectory Slider: collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1680 - X: -10 - Y: 21 + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch b/ur5_moveit_config/launch/moveit_planning_execution.launch similarity index 58% rename from ur5_moveit_config/launch/ur5_moveit_planning_execution.launch rename to ur5_moveit_config/launch/moveit_planning_execution.launch index d2d697711..3022d867d 100644 --- a/ur5_moveit_config/launch/ur5_moveit_planning_execution.launch +++ b/ur5_moveit_config/launch/moveit_planning_execution.launch @@ -1,14 +1,12 @@ - - + - - + + - diff --git a/ur5_moveit_config/launch/moveit_rviz.launch b/ur5_moveit_config/launch/moveit_rviz.launch index c8216c755..45ffce7c5 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/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/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..3b66fc54d 100644 --- a/ur5_moveit_config/package.xml +++ b/ur5_moveit_config/package.xml @@ -17,17 +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 - + diff --git a/ur5_moveit_config/tests/moveit_planning_execution.xml b/ur5_moveit_config/tests/moveit_planning_execution.xml index d566fa9db..16052aa74 100644 --- a/ur5_moveit_config/tests/moveit_planning_execution.xml +++ b/ur5_moveit_config/tests/moveit_planning_execution.xml @@ -1,16 +1,14 @@ - - - + - - + + 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/ur10_e_moveit_config/CMakeLists.txt b/ur5e_moveit_config/CMakeLists.txt similarity index 92% rename from ur10_e_moveit_config/CMakeLists.txt rename to ur5e_moveit_config/CMakeLists.txt index 408778222..0c83d4541 100644 --- a/ur10_e_moveit_config/CMakeLists.txt +++ b/ur5e_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ur10_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/ur10_e_moveit_config/config/controllers.yaml b/ur5e_moveit_config/config/fake_controllers.yaml similarity index 59% rename from ur10_e_moveit_config/config/controllers.yaml rename to ur5e_moveit_config/config/fake_controllers.yaml index bf2252cb2..afbfa6e6c 100644 --- a/ur10_e_moveit_config/config/controllers.yaml +++ b/ur5e_moveit_config/config/fake_controllers.yaml @@ -1,7 +1,5 @@ controller_list: - - name: "" - action_ns: follow_joint_trajectory - type: FollowJointTrajectory + - name: fake_manipulator_controller joints: - shoulder_pan_joint - shoulder_lift_joint @@ -9,3 +7,6 @@ controller_list: - wrist_1_joint - wrist_2_joint - wrist_3_joint +initial: # Define initial robot poses. + - group: manipulator + pose: home diff --git a/ur5e_moveit_config/config/joint_limits.yaml b/ur5e_moveit_config/config/joint_limits.yaml new file mode 100644 index 000000000..bdc914f89 --- /dev/null +++ b/ur5e_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: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + 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 diff --git a/ur5e_moveit_config/config/kinematics.yaml b/ur5e_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..51e29f53e --- /dev/null +++ b/ur5e_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: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + solve_type: Distance + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 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..39ba9f166 --- /dev/null +++ b/ur5e_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,149 @@ +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: + longest_valid_segment_fraction: 0.01 + default_planner_config: RRTConnect + 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..bfc4186c4 --- /dev/null +++ b/ur5e_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +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 82% rename from ur5_e_moveit_config/config/ur5e.srdf rename to ur5e_moveit_config/config/ur5e.srdf index bb009a906..daab73c7b 100644 --- a/ur5_e_moveit_config/config/ur5e.srdf +++ b/ur5e_moveit_config/config/ur5e.srdf @@ -3,17 +3,14 @@ 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 --> - + - - - - + @@ -32,18 +29,17 @@ - - - - - - + + + + + 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..eef307e03 --- /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/ur5e_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/ur5e_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..0e7d73907 --- /dev/null +++ b/ur5e_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/ur5e_moveit_config/launch/move_group.launch b/ur5e_moveit_config/launch/move_group.launch new file mode 100644 index 000000000..e55b731ed --- /dev/null +++ b/ur5e_moveit_config/launch/move_group.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/moveit.rviz b/ur5e_moveit_config/launch/moveit.rviz new file mode 100644 index 000000000..cfc2fec04 --- /dev/null +++ b/ur5e_moveit_config/launch/moveit.rviz @@ -0,0 +1,280 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5175438523292542 + Tree Height: 259 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + 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 + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + 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: + 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 + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + 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 + Trail Step Size: 1 + 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.07999999821186066 + 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: 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: 1 + Scene Color: 50; 230; 50 + 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: + 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 + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + 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: 0.5 + 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 + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + 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.009999999776482582 + Pitch: 0.45979705452919006 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 0.9717636108398438 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 783 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001ca000002b9fc0200000007fb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017f000001750000016a00ffffff000002ce000002b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1182 + X: 481 + Y: 253 diff --git a/ur5e_moveit_config/launch/moveit_planning_execution.launch b/ur5e_moveit_config/launch/moveit_planning_execution.launch new file mode 100644 index 000000000..82a6e2105 --- /dev/null +++ b/ur5e_moveit_config/launch/moveit_planning_execution.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/moveit_rviz.launch b/ur5e_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..42c17cd43 --- /dev/null +++ b/ur5e_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/ompl_planning_pipeline.launch.xml b/ur5e_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 000000000..7ec6880f1 --- /dev/null +++ b/ur5e_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/planning_context.launch b/ur5e_moveit_config/launch/planning_context.launch new file mode 100644 index 000000000..9052fd96f --- /dev/null +++ b/ur5e_moveit_config/launch/planning_context.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/planning_pipeline.launch.xml b/ur5e_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 000000000..caf1be307 --- /dev/null +++ b/ur5e_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/ur5e_moveit_config/launch/run_benchmark_ompl.launch b/ur5e_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 000000000..757bc9c52 --- /dev/null +++ b/ur5e_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/sensor_manager.launch.xml b/ur5e_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 000000000..97df2bd8f --- /dev/null +++ b/ur5e_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + 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/ur5e_moveit_config/launch/trajectory_execution.launch.xml b/ur5e_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 000000000..bbad585bf --- /dev/null +++ b/ur5e_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + 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/ur5e_moveit_config/launch/ur5e_moveit_sensor_manager.launch.xml b/ur5e_moveit_config/launch/ur5e_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/ur5e_moveit_config/launch/ur5e_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/ur5e_moveit_config/launch/warehouse.launch b/ur5e_moveit_config/launch/warehouse.launch new file mode 100644 index 000000000..4ec8b7786 --- /dev/null +++ b/ur5e_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/ur5e_moveit_config/launch/warehouse_settings.launch.xml b/ur5e_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 000000000..e473b083b --- /dev/null +++ b/ur5e_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/ur5_e_moveit_config/package.xml b/ur5e_moveit_config/package.xml similarity index 73% rename from ur5_e_moveit_config/package.xml rename to ur5e_moveit_config/package.xml index f81c4cc1d..23c6c407c 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,22 +10,30 @@ 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 + 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 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..1a256e673 --- /dev/null +++ b/ur5e_moveit_config/tests/moveit_planning_execution.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + +