From bb0cfd40f47026871945176b6dd71f1d90233abc Mon Sep 17 00:00:00 2001 From: ValsCSGO Date: Tue, 7 Sep 2021 01:49:41 -0400 Subject: [PATCH] Moveit is pain --- src/ar3_config/config/ar3_controllers.yaml | 15 ++ src/ar3_config/config/kinematics.yaml | 4 + src/ar3_config/config/ompl_planning.yaml | 149 ++++++++++++++++++ src/ar3_config/launch/dev.launch.py | 63 +++++++- .../ar3_description/state_publisher.py | 2 +- src/ar3_description/urdf/ar3.srdf | 8 + 6 files changed, 237 insertions(+), 4 deletions(-) create mode 100644 src/ar3_config/config/ar3_controllers.yaml create mode 100644 src/ar3_config/config/kinematics.yaml create mode 100644 src/ar3_config/config/ompl_planning.yaml create mode 100644 src/ar3_description/urdf/ar3.srdf diff --git a/src/ar3_config/config/ar3_controllers.yaml b/src/ar3_config/config/ar3_controllers.yaml new file mode 100644 index 0000000..80522ca --- /dev/null +++ b/src/ar3_config/config/ar3_controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - joint_trajectory_controller + +joint_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 diff --git a/src/ar3_config/config/kinematics.yaml b/src/ar3_config/config/kinematics.yaml new file mode 100644 index 0000000..9eae252 --- /dev/null +++ b/src/ar3_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +ar3_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 diff --git a/src/ar3_config/config/ompl_planning.yaml b/src/ar3_config/config/ompl_planning.yaml new file mode 100644 index 0000000..0640f3d --- /dev/null +++ b/src/ar3_config/config/ompl_planning.yaml @@ -0,0 +1,149 @@ +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 + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + +ar3_arm: + # default_planner_config: RRTConnect #RRTConnectkConfigDefault + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index 886a5f7..4d9b2a4 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -3,6 +3,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import ExecuteProcess +import yaml def generate_launch_description(): launch_root = get_package_share_directory('ar3_config') @@ -13,6 +14,46 @@ def generate_launch_description(): with open(urdf_path, 'r') as f: robot_description = {'robot_description': f.read()} + srdf_path = join(description_root, 'ar3.srdf') + with open(srdf_path, 'r') as f: + robot_description_semantic = {'robot_description_semantic': f.read()} + + robot_description_kinematics = {} + with open(join(config_root, 'kinematics.yaml'), 'r') as f: + robot_description_kinematics.update({"robot_description_kinematics": yaml.safe_load(f)}) + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" , + "start_state_max_bounds_error": 0.1, + } + } + + with open(join(config_root, "ompl_planning.yaml"), 'r') as f: + ompl_planning_pipeline_config["move_group"].update(yaml.safe_load(f)) + + # Trajectory Execution Functionality + moveit_controllers = { + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager" + } + with open(join(config_root, "ar3_controllers.yaml"), 'r') as f: + moveit_controllers.update({"moveit_simple_controller_manager": yaml.safe_load(f)}) + + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + return LaunchDescription([ # RViz Node( @@ -27,7 +68,7 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link']), + arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']), # Controller Node( @@ -46,6 +87,22 @@ def generate_launch_description(): shell=True, output='screen'), + # Start the actual move_group node/action server + Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters + ], + ), + # Robot state publisher Node( package='robot_state_publisher', @@ -56,8 +113,8 @@ def generate_launch_description(): # Fake robot state publisher Node( package='ar3_description', - executable='state_publisher_net', - name='state_publisher_net', + executable='state_publisher', + name='state_publisher', emulate_tty=True, output='screen'), ]) diff --git a/src/ar3_description/ar3_description/state_publisher.py b/src/ar3_description/ar3_description/state_publisher.py index f628aa4..5fb602d 100644 --- a/src/ar3_description/ar3_description/state_publisher.py +++ b/src/ar3_description/ar3_description/state_publisher.py @@ -31,7 +31,7 @@ class StatePublisher(Node): # message declarations odom_trans = TransformStamped() - odom_trans.header.frame_id = 'odom' + odom_trans.header.frame_id = 'world' odom_trans.child_frame_id = 'base_link' joint_state = JointState() diff --git a/src/ar3_description/urdf/ar3.srdf b/src/ar3_description/urdf/ar3.srdf new file mode 100644 index 0000000..ab900e2 --- /dev/null +++ b/src/ar3_description/urdf/ar3.srdf @@ -0,0 +1,8 @@ + + + + + + + +