Merge pull request #4 from LCAS/humble

PILZ planner integration
yilmazabdurrah · Oct 1, 2024 · 5d51439 · 5d51439
2 parents 1b2d93f + ab71d88
commit 5d51439
Show file tree

Hide file tree

Showing 3 changed files with 74 additions and 18 deletions.
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
@@ -66,7 +66,7 @@
       <parent link="${arm_id}_link0"/>
       <child link="${arm_id}_link1"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.7473" upper="2.7473" velocity="2.1750"/>
+      <limit effort="87" lower="-2.7473" upper="2.7473" velocity="2.1750" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link2">
       <visual>
@@ -99,7 +99,7 @@
       <parent link="${arm_id}_link1"/>
       <child link="${arm_id}_link2"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-1.6728" upper="1.6728" velocity="2.1750"/>
+      <limit effort="87" lower="-1.6728" upper="1.6728" velocity="2.1750" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link3">
       <visual>
@@ -132,7 +132,7 @@
       <parent link="${arm_id}_link2"/>
       <child link="${arm_id}_link3"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.7473" upper="2.7473" velocity="2.1750"/>
+      <limit effort="87" lower="-2.7473" upper="2.7473" velocity="2.1750" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link4">
       <visual>
@@ -165,7 +165,7 @@
       <parent link="${arm_id}_link3"/>
       <child link="${arm_id}_link4"/>
       <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.9218" upper="-0.2198" velocity="2.1750"/>
+      <limit effort="87" lower="-2.9218" upper="-0.2198" velocity="2.1750" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link5">
       <visual>
@@ -217,7 +217,7 @@
       <parent link="${arm_id}_link4"/>
       <child link="${arm_id}_link5"/>
       <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-2.7473" upper="2.7473" velocity="2.6100"/>
+      <limit effort="12" lower="-2.7473" upper="2.7473" velocity="2.6100" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link6">
       <visual>
@@ -250,7 +250,7 @@
       <parent link="${arm_id}_link5"/>
       <child link="${arm_id}_link6"/>
       <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-0.0775" upper="3.6025" velocity="2.6100"/>
+      <limit effort="12" lower="-0.0775" upper="3.6025" velocity="2.6100" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link7">
       <visual>
@@ -283,7 +283,7 @@
       <parent link="${arm_id}_link6"/>
       <child link="${arm_id}_link7"/>
       <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-2.7473" upper="2.7473" velocity="2.6100"/>
+      <limit effort="12" lower="-2.7473" upper="2.7473" velocity="2.6100" acceleration="1.0"/>
     </joint>
     <link name="${arm_id}_link8">
       <collision>

diff --git a/franka_moveit_config/config/ompl_planning.yaml b/franka_moveit_config/config/ompl_planning.yaml
@@ -132,16 +132,39 @@ planner_configs:
     range: 0.0
     goal_bias: 0.05
     delay_collision_checking: 1
-  pilz_lin:
+  PILZ_LIN:
     type: pilz_industrial_motion_planner/Linear
+    planner_id: "LIN"
     planning_time: 10.0
     max_velocity_scaling_factor: 0.5
     max_acceleration_scaling_factor: 0.5
-  pilz_circ:
+  PILZ_CIRC:
     type: pilz_industrial_motion_planner/Circular
+    planner_id: "CIRC"
     planning_time: 10.0
     max_velocity_scaling_factor: 0.5
     max_acceleration_scaling_factor: 0.5
+  PILZ_PTP:
+    type: pilz_industrial_motion_planner/Circular
+    planner_id: "PTP"
+    planning_time: 10.0
+    max_velocity_scaling_factor: 0.5
+    max_acceleration_scaling_factor: 0.5
+
+robot_description_planning:
+  cartesian_limits:
+    max_trans_vel: 0.2        # Maximum translational velocity (m/s)
+    max_trans_acc: 0.5        # Maximum translational acceleration (m/s^2)
+    max_trans_dec: 0.5        # Maximum translational deceleration (m/s^2)
+    max_rot_vel: 1.0          # Maximum rotational velocity (rad/s)
+
+planning_pipelines:
+  default:
+    planner_configs: OMPL
+  ompl:
+    planner_configs: OMPL
+  pilz:
+    planner_configs: [PILZ_LIN, PILZ_CIRC, PILZ_PTP]
 
 panda_arm:
   enforce_constrained_state_space: true
@@ -171,8 +194,9 @@ panda_arm:
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
     - TrajOptDefault
-    - pilz_lin  
-    - pilz_circ
+    - PILZ_LIN  
+    - PILZ_CIRC
+    - OMPL
 panda_arm_hand:
   enforce_constrained_state_space: true
   projection_evaluator: joints(panda_joint1, panda_joint2)
@@ -201,8 +225,9 @@ panda_arm_hand:
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
     - TrajOptDefault
-    - pilz_lin  
-    - pilz_circ
+    - PILZ_LIN  
+    - PILZ_CIRC
+    - OMPL
 panda_vision:
   enforce_constrained_state_space: true
   projection_evaluator: joints(panda_joint1, panda_joint2)
@@ -231,6 +256,7 @@ panda_vision:
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
     - TrajOptDefault
-    - pilz_lin  
-    - pilz_circ
+    - PILZ_LIN  
+    - PILZ_CIRC
+    - OMPL
 
diff --git a/franka_moveit_config/launch/moveit_real_arm_platform.launch.py b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py
@@ -46,6 +46,7 @@ def generate_launch_description():
     load_gripper_parameter_name = 'load_gripper'
     fake_sensor_commands_parameter_name = 'fake_sensor_commands'
     load_camera_parameter_name = 'load_camera'
+    planner_parameter_name = 'planner'
 
     camera_id_parameter_name = 'serial'
     camera_model_parameter_name = 'camera_type'
@@ -59,6 +60,8 @@ def generate_launch_description():
     camera_id = LaunchConfiguration(camera_id_parameter_name)
     camera_model = LaunchConfiguration(camera_model_parameter_name)
 
+    planner_ = LaunchConfiguration(planner_parameter_name)
+
     # Command-line arguments
     db_arg = DeclareLaunchArgument(
         'db', default_value='False', description='Database flag'
@@ -149,9 +152,32 @@ def generate_launch_description():
 
     combined_planning_pipelines_config = {
         'move_group': {
-            'planning_pipelines': 'ompl, pilz',
+            'planning_plugin': planner_,
+            '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'
+                                'default_planning_request_adapters/CheckStartStateBounds'
+                                'default_planning_request_adapters/CheckStartStateCollision'
+                                'default_planning_request_adapters/ValidateWorkspaceBounds',
+            'start_state_max_bounds_error': 0.1,
+            'default_planner_config': 'PTP',
+        },
+        'robot_description_planning':{
+            'cartesian_limits': {
+                'max_trans_vel': 0.2,
+                'max_trans_acc': 1.0,
+                'max_trans_dec': -1.0,
+                'max_rot_vel': 0.5
+            }
         }
     }
+    combined_planning_yaml = load_yaml(
+        'franka_moveit_config', 'config/ompl_planning.yaml'
+    )
+    combined_planning_pipelines_config['move_group'].update(combined_planning_yaml)
 
     # Start the actual move_group node/action server
     run_move_group_node = Node(
@@ -163,8 +189,6 @@ def generate_launch_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,
@@ -262,6 +286,11 @@ def generate_launch_description():
             default_value='true',
             description='Use Flir camera as an end-effector, otherwise, the robot is loaded '
                         'without an end-effector.')  
+
+    planner_arg = DeclareLaunchArgument(
+            planner_parameter_name,
+            default_value='ompl_interface/OMPLPlanner',
+            description='Choose planner to be used for arm control ') 
 
     load_gripper_arg = DeclareLaunchArgument(
             load_gripper_parameter_name,
@@ -307,6 +336,7 @@ def generate_launch_description():
          fake_sensor_commands_arg,
          load_gripper_arg,
          load_camera_arg,
+         planner_arg,
          camera_id_arg,
          camera_model_arg,
          db_arg,