diff --git a/franka_moveit_config/config/ompl_planning.yaml b/franka_moveit_config/config/ompl_planning.yaml index 0a71e81..1dc2b7b 100644 --- a/franka_moveit_config/config/ompl_planning.yaml +++ b/franka_moveit_config/config/ompl_planning.yaml @@ -122,6 +122,26 @@ planner_configs: max_failures: 5000 # maximum consecutive failure limit. default: 5000 TrajOptDefault: type: geometric::TrajOpt + RRTsharpkConfigDefault: + type: geometric::RRTsharp + 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 + RRTXstatickConfigDefault: + type: geometric::RRTXstatic + range: 0.0 + goal_bias: 0.05 + delay_collision_checking: 1 + pilz_lin: + type: pilz_industrial_motion_planner/Linear + planning_time: 10.0 + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 0.5 + pilz_circ: + type: pilz_industrial_motion_planner/Circular + planning_time: 10.0 + max_velocity_scaling_factor: 0.5 + max_acceleration_scaling_factor: 0.5 panda_arm: enforce_constrained_state_space: true @@ -151,6 +171,8 @@ panda_arm: - SPARSkConfigDefault - SPARStwokConfigDefault - TrajOptDefault + - pilz_lin + - pilz_circ panda_arm_hand: enforce_constrained_state_space: true projection_evaluator: joints(panda_joint1, panda_joint2) @@ -179,6 +201,8 @@ panda_arm_hand: - SPARSkConfigDefault - SPARStwokConfigDefault - TrajOptDefault + - pilz_lin + - pilz_circ panda_vision: enforce_constrained_state_space: true projection_evaluator: joints(panda_joint1, panda_joint2) @@ -207,4 +231,6 @@ panda_vision: - SPARSkConfigDefault - SPARStwokConfigDefault - TrajOptDefault + - pilz_lin + - pilz_circ diff --git a/franka_moveit_config/launch/moveit_real_arm_platform.launch.py b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py index 2caa019..9f54987 100644 --- a/franka_moveit_config/launch/moveit_real_arm_platform.launch.py +++ b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py @@ -106,6 +106,23 @@ def generate_launch_description(): ) ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + pilz_planning_pipeline_config = { + 'move_group': { + 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', + 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization ' + 'default_planner_request_adapters/ResolveConstraintFrames ' + '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, + } + } + pilz_planning_yaml = load_yaml( + 'franka_moveit_config', 'config/ompl_planning.yaml' + ) + pilz_planning_pipeline_config['move_group'].update(pilz_planning_yaml) + # Trajectory Execution Functionality moveit_simple_controllers_yaml = load_yaml( 'franka_moveit_config', 'config/panda_controllers.yaml' @@ -130,6 +147,12 @@ def generate_launch_description(): 'publish_transforms_updates': True, } + combined_planning_pipelines_config = { + 'move_group': { + 'planning_pipelines': 'ompl, pilz', + } + } + # Start the actual move_group node/action server run_move_group_node = Node( package='moveit_ros_move_group', @@ -139,7 +162,9 @@ def generate_launch_description(): robot_description, robot_description_semantic, kinematics_yaml, + combined_planning_pipelines_config, ompl_planning_pipeline_config, + pilz_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, diff --git a/franka_moveit_config/rviz/moveit.rviz b/franka_moveit_config/rviz/moveit.rviz index c10e62c..5c2aba6 100644 --- a/franka_moveit_config/rviz/moveit.rviz +++ b/franka_moveit_config/rviz/moveit.rviz @@ -1,16 +1,18 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /MarkerArray1 - /PlanningScene1 - /Pose1 - - /TF1 - - /TF1/Frames1 + - /Image1 + - /Image1/Topic1 + - /PoseArray1 Splitter Ratio: 0.5 - Tree Height: 907 + Tree Height: 632 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -365,11 +367,11 @@ Visualization Manager: Frames: All Enabled: false camera: - Value: true + Value: false camera_lens: Value: false camera_optical: - Value: false + Value: true panda_link0: Value: true panda_link1: @@ -387,35 +389,69 @@ Visualization Manager: panda_link7: Value: false panda_link8: - Value: true + Value: false plant: - Value: true + Value: false Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - panda_link0: - panda_link1: - panda_link2: - panda_link3: - panda_link4: - panda_link5: - panda_link6: - panda_link7: - panda_link8: - camera: - camera_lens: - camera_optical: - {} plant: - {} + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + camera: + camera_lens: + camera_optical: + {} Update Interval: 0 Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /image_color + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lattice_pose_array + Value: false Enabled: true Global Options: - Background Color: 48; 48; 48 + Background Color: 255; 255; 255 Fixed Frame: panda_link0 Frame Rate: 30 Name: root @@ -459,25 +495,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.655773401260376 + Distance: 1.6765280961990356 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.02386285550892353 - Y: 0.15478567779064178 - Z: 0.039489321410655975 + X: 0.37200048565864563 + Y: -0.2571171820163727 + Z: 0.049729373306035995 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7403981685638428 + Pitch: 0.46040067076683044 Target Frame: Value: Orbit (rviz) - Yaw: 5.028563022613525 + Yaw: 2.2366816997528076 Saved: ~ Window Geometry: Displays: @@ -485,11 +521,13 @@ Window Geometry: Height: 1136 Hide Left Dock: false Hide Right Dock: true + Image: + collapsed: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: