Merge pull request #3 from LCAS/humble

PILZ planner
yilmazabdurrah · Sep 30, 2024 · 1b2d93f · 1b2d93f
2 parents a7f0832 + 86d08b0
commit 1b2d93f
Show file tree

Hide file tree

Showing 3 changed files with 119 additions and 30 deletions.
diff --git 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
@@ -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
@@ -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,37 +495,39 @@ 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: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.028563022613525
+      Yaw: 2.2366816997528076
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   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: 000000ff00000000fd0000000400000000000001f300000416fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000223000001d30000017d00fffffffb0000000a0049006d00610067006501000002f80000015b0000002800fffffffb0000000a0049006d00610067006501000002e3000001700000000000000000000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005410000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties: