Repository: MAGIC-MRSD26/magic
Pull request #22: added planner, customization, and default as RRT Connect
Author: login: mhon-emma
Created: 2025-04-04T01:05:13Z | Merged: 2025-04-06T03:32:24Z



Diff:
diff --git a/ros2_ws/src/two_arm_moveit2_config/config/ompl_planning.yaml b/ros2_ws/src/two_arm_moveit2_config/config/ompl_planning.yaml
new file mode 100644
index 00000000..cdf5ce16
--- /dev/null
+++ b/ros2_ws/src/two_arm_moveit2_config/config/ompl_planning.yaml
@@ -0,0 +1,153 @@
+planning_plugin: ompl_interface/OMPLPlanner
+request_adapters: >-
+  default_planner_request_adapters/AddTimeOptimalParameterization
+  default_planner_request_adapters/ResolveConstraintFrames
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+
+# Configuration for the 'both_arms' group
+both_arms:
+  planner_configs:
+    - RRTConnect
+    - RRT
+    - RRTstar
+    - BiTRRT
+    - LazyPRM
+  default_planner_config: RRTConnect
+  # Goal sampling parameters
+  goal_joint_tolerance: 0.01
+  goal_position_tolerance: 0.01
+  goal_orientation_tolerance: 0.01
+  
+  # Using left and right arm joints for projection
+  projection_evaluator: joints(left_joint_1,right_joint_1)
+  longest_valid_segment_fraction: 0.005
+  simplify_solutions: true
+
+# Configuration for the 'both_arms_with_grippers' group
+both_arms_with_grippers:
+  planner_configs:
+    - RRTConnect
+    - RRT
+    - RRTstar
+    - BiTRRT
+    - LazyPRM
+  default_planner_config: RRTConnect
+  
+  goal_joint_tolerance: 0.01
+  goal_position_tolerance: 0.01
+  goal_orientation_tolerance: 0.01
+  projection_evaluator: joints(left_joint_1,right_joint_1)
+  longest_valid_segment_fraction: 0.005
+  simplify_solutions: true
+
+# Configuration for the 'left_arm' group
+left_arm:
+  planner_configs:
+    - RRTConnect
+    - RRT
+    - RRTstar
+    - BiTRRT
+    - LazyPRM
+  default_planner_config: RRTConnect
+  goal_joint_tolerance: 0.01
+  goal_position_tolerance: 0.01
+  goal_orientation_tolerance: 0.01
+  projection_evaluator: joints(left_joint_1,left_joint_2)
+  longest_valid_segment_fraction: 0.005
+
+# Configuration for the 'right_arm' group
+right_arm:
+  planner_configs:
+    - RRTConnect
+    - RRT
+    - RRTstar
+    - BiTRRT
+    - LazyPRM
+  default_planner_config: RRTConnect
+  goal_joint_tolerance: 0.01
+  goal_position_tolerance: 0.01
+  goal_orientation_tolerance: 0.01
+  projection_evaluator: joints(right_joint_1,right_joint_2)
+  longest_valid_segment_fraction: 0.005
+
+# Configuration for the 'left_arm_with_gripper' group
+left_arm_with_gripper:
+  planner_configs:
+    - RRTConnect
+    - RRT
+    - RRTstar
+    - BiTRRT
+    - LazyPRM
+  default_planner_config: RRTConnect
+  goal_joint_tolerance: 0.01
+  goal_position_tolerance: 0.01
+  goal_orientation_tolerance: 0.01
+  projection_evaluator: joints(left_joint_1,left_joint_2)
+  longest_valid_segment_fraction: 0.005
+
+# Configuration for the 'right_arm_with_gripper' group
+right_arm_with_gripper:
+  planner_configs:
+    - RRTConnect
+    - RRT
+    - RRTstar
+    - BiTRRT
+    - LazyPRM
+  default_planner_config: RRTConnect
+  goal_joint_tolerance: 0.01
+  goal_position_tolerance: 0.01
+  goal_orientation_tolerance: 0.01
+  projection_evaluator: joints(right_joint_1,right_joint_2)
+  longest_valid_segment_fraction: 0.005
+
+# Planner configurations
+RRTConnect:
+  type: geometric::RRTConnect
+  range: 0.0  # Max step size (0.0 means auto)
+  goal_bias: 0.1  # Increased from 0.05 to help with goal sampling
+
+RRT:
+  type: geometric::RRT
+  range: 0.0
+  goal_bias: 0.1
+
+RRTstar:
+  type: geometric::RRTstar
+  range: 0.0
+  goal_bias: 0.1
+  delay_collision_checking: 1
+  max_states_failed: 100
+  
+TRRTConnect:
+  type: geometric::TRRTConnect
+  range: 0.0
+  goal_bias: 0.1
+
+BiTRRT:
+  type: geometric::BiTRRT
+  range: 0.0
+  temp_change_factor: 0.1
+  init_temperature: 100
+  cost_threshold: 1e300
+
+LazyPRM:
+  type: geometric::LazyPRM
+  range: 0.0
+  max_nearest_neighbors: 5
+
+PRMstar:
+  type: geometric::PRMstar
+  range: 0.0
+
+TRRT:
+  type: geometric::TRRT
+  range: 0.0
+  goal_bias: 0.1
+  temp_change_factor: 0.1
+  min_temperature: 10e-10
+  init_temperature: 10e-6
+  frountier_threshold: 0.0
+  frountier_node_ratio: 0.1
+  k_constant: 0.0
\ No newline at end of file
diff --git a/ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py b/ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py
index 1191be09..8be261e4 100644
--- a/ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py
+++ b/ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py
@@ -43,7 +43,7 @@ def launch_setup(context, *args, **kwargs):
         .robot_description(mappings=launch_arguments)
         .trajectory_execution(file_path="config/moveit_controllers.yaml")
         .planning_pipelines(
-            pipelines=["ompl", "pilz_industrial_motion_planner"],
+            pipelines=["ompl", "pilz_industrial_motion_planner", "chomp"],
             default_planning_pipeline="ompl"
         )
         .planning_scene_monitor(
@@ -53,6 +53,20 @@ def launch_setup(context, *args, **kwargs):
         .to_moveit_configs()
     )  
 
+    ompl_planning_yaml = os.path.join(
+    get_package_share_directory("two_arm_moveit2_config"),
+    "config",
+    "ompl_planning.yaml"
+    )
+    if os.path.exists(ompl_planning_yaml):
+        with open(ompl_planning_yaml, 'r') as f:
+            ompl_config = yaml.safe_load(f)
+            if not hasattr(moveit_config, 'planning_pipelines'):
+                moveit_config.planning_pipelines = {}
+            if 'ompl' not in moveit_config.planning_pipelines:
+                moveit_config.planning_pipelines['ompl'] = {}
+            moveit_config.planning_pipelines['ompl'].update(ompl_config)
+
     moveit_config.moveit_cpp.update({"use_sim_time": use_sim_time.perform(context) == "true"})
 
     move_group_node = Node(
@@ -61,6 +75,8 @@ def launch_setup(context, *args, **kwargs):
         output="screen",
         parameters=[
             moveit_config.to_dict(),
+            {"planning_pipeline": "ompl", 
+            "default_planner_id": "RRTConnect"} 
         ],
     )
 
diff --git a/ros2_ws/src/two_arm_moveit2_config/urdf/gen3_dual_arm.xacro b/ros2_ws/src/two_arm_moveit2_config/urdf/gen3_dual_arm.xacro
index a4c52eeb..541d114b 100644
--- a/ros2_ws/src/two_arm_moveit2_config/urdf/gen3_dual_arm.xacro
+++ b/ros2_ws/src/two_arm_moveit2_config/urdf/gen3_dual_arm.xacro
@@ -33,6 +33,10 @@
     <color rgba="0.0 0.1 0.2 1"/>
     </material>
 
+    <material name="transparent">
+    <color rgba="0.8 0.8 0.8 0"/>
+    </material>
+
     <material name="bin_material">
     <color rgba="0.25 0.25 0.25 1"/>
     </material>
@@ -135,13 +139,13 @@
     <link name="bound_top_link">
     <visual>
         <geometry>
-        <box size="1.5 0.1 0.1"/>
+        <box size="1.5 0.06 0.06"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
-        <box size="1.5 0.1 0.1"/>
+        <box size="1.5 0.06 0.06"/>
         </geometry>
     </collision>
     <inertial>
@@ -165,13 +169,13 @@
     <link name="bound_left_link">
     <visual>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
     </collision>
     <inertial>
@@ -196,13 +200,13 @@
     <link name="bound_right_link">
     <visual>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
     </collision>
     <inertial>
@@ -220,19 +224,19 @@
     <joint name="bound_right_joint" type="fixed">
     <parent link="world"/>
     <child link="bound_right_link"/>
-    <origin rpy="0 0 1.5708" xyz="-0.7 0 1.8"/>
+    <origin rpy="0 0 1.5708" xyz="-0.73 0 1.8"/>
     </joint>
 
     <link name="bound_right_top_link">
     <visual>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
     </collision>
     <inertial>
@@ -250,26 +254,26 @@
     <joint name="bound_right_top_joint" type="fixed">
     <parent link="world"/>
     <child link="bound_right_top_link"/>
-    <origin rpy="0 0 1.5708" xyz="-0.7 0 2.03"/>
+    <origin rpy="0 0 1.5708" xyz="-0.73 0 2.03"/>
     </joint>
 
     <!-- Simple box link -->
     <link name="bound_left_top_link">
     <visual>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
-        <box size="1 0.1 0.1"/>
+        <box size="1 0.06 0.06"/>
         </geometry>
     </collision>
     <inertial>
         <mass value="1.0"/>
         <inertia 
-        ixx="0.00167" 
+        ixx="0.0" 
         ixy="0.0" 
         ixz="0.0" 
         iyy="0.33417" 
@@ -290,7 +294,7 @@
         <geometry>
         <box size="2 0.1 0.1"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
@@ -321,7 +325,7 @@
         <geometry>
         <box size="2 0.1 0.1"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
@@ -343,7 +347,7 @@
     <joint name="bound_stand_2_joint" type="fixed">
     <parent link="world"/>
     <child link="bound_stand_2_link"/>
-    <origin rpy="0 1.5708 0" xyz="-0.7 -0.55 1.1"/>
+    <origin rpy="0 1.5708 0" xyz="-0.73 -0.55 1.1"/>
     </joint>
 
     <link name="bound_stand_3_link">
@@ -351,7 +355,7 @@
         <geometry>
         <box size="2 0.1 0.1"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>
@@ -373,7 +377,7 @@
     <joint name="bound_stand_3_joint" type="fixed">
     <parent link="world"/>
     <child link="bound_stand_3_link"/>
-    <origin rpy="0 1.5708 0" xyz="-0.7 0.55 1.1"/>
+    <origin rpy="0 1.5708 0" xyz="-0.73 0.55 1.1"/>
     </joint>
 
     <link name="bound_stand_4_link">
@@ -381,7 +385,7 @@
         <geometry>
         <box size="2 0.1 0.1"/>
         </geometry>
-        <material name="dark_blue_metal"/>
+        <material name="transparent"/>
     </visual>
     <collision>
         <geometry>


Reviews:
user: login: meganmlee
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-04-04T01:47:09Z
user: login: mhon-emma
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-04T01:52:58Z
user: login: meganmlee
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-04-04T01:54:22Z
user: login: meganmlee
state: APPROVED
author_association: COLLABORATOR
submitted_at: 2025-04-06T01:30:50Z

Review comments:
diff_hunk: @@ -53,6 +53,20 @@ def launch_setup(context, *args, **kwargs):
         .to_moveit_configs()
     )  
 
+    ompl_planning_yaml = os.path.join(
+    get_package_share_directory("two_arm_moveit2_config"),
+    "config",
+    "ompl_planning.yaml"
+    )
+    if os.path.exists(ompl_planning_yaml):
+        with open(ompl_planning_yaml, 'r') as f:
+            ompl_config = yaml.safe_load(f)
+            if not hasattr(moveit_config, 'planning_pipelines'):
+                moveit_config.planning_pipelines = {}
+            if 'ompl' not in moveit_config.planning_pipelines:
+                moveit_config.planning_pipelines['ompl'] = {}
+            moveit_config.planning_pipelines['ompl'].update(ompl_config)
+
path: ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py
user: login: meganmlee
body: what does this do?
created_at: 2025-04-04T01:47:09Z
author_association: COLLABORATOR
diff_hunk: @@ -53,6 +53,20 @@ def launch_setup(context, *args, **kwargs):
         .to_moveit_configs()
     )  
 
+    ompl_planning_yaml = os.path.join(
+    get_package_share_directory("two_arm_moveit2_config"),
+    "config",
+    "ompl_planning.yaml"
+    )
+    if os.path.exists(ompl_planning_yaml):
+        with open(ompl_planning_yaml, 'r') as f:
+            ompl_config = yaml.safe_load(f)
+            if not hasattr(moveit_config, 'planning_pipelines'):
+                moveit_config.planning_pipelines = {}
+            if 'ompl' not in moveit_config.planning_pipelines:
+                moveit_config.planning_pipelines['ompl'] = {}
+            moveit_config.planning_pipelines['ompl'].update(ompl_config)
+
path: ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py
user: login: mhon-emma
body: This loads the ompl planning yaml file correctly to get the different planners. If we only pull the ompl (line 46), moveit2 only uses the default or what it thinks is the best, which is usually RRT without any customization. 
created_at: 2025-04-04T01:52:58Z
author_association: CONTRIBUTOR
diff_hunk: @@ -53,6 +53,20 @@ def launch_setup(context, *args, **kwargs):
         .to_moveit_configs()
     )  
 
+    ompl_planning_yaml = os.path.join(
+    get_package_share_directory("two_arm_moveit2_config"),
+    "config",
+    "ompl_planning.yaml"
+    )
+    if os.path.exists(ompl_planning_yaml):
+        with open(ompl_planning_yaml, 'r') as f:
+            ompl_config = yaml.safe_load(f)
+            if not hasattr(moveit_config, 'planning_pipelines'):
+                moveit_config.planning_pipelines = {}
+            if 'ompl' not in moveit_config.planning_pipelines:
+                moveit_config.planning_pipelines['ompl'] = {}
+            moveit_config.planning_pipelines['ompl'].update(ompl_config)
+
path: ros2_ws/src/two_arm_moveit2_config/launch/robot.launch.py
user: login: meganmlee
body: ![image](https://github.com/user-attachments/assets/dc19ca9c-5bbf-480f-9bb3-019cabb429c2)
when i load rviz it still shows unspecified. How do i know RRTconnect is the one its on?

I just tested my script on it and the path was weird which is why i ask
created_at: 2025-04-04T01:54:22Z
author_association: COLLABORATOR

PR comments:
user: login: meganmlee
created_at: 2025-04-06T01:30:11Z
body: could we change this to RRTStar as default? Ive noticed it performs better.

feel free to merge after that!
author_association: COLLABORATOR
user: login: meganmlee
created_at: 2025-04-06T01:37:20Z
body: also i tried changing goal bias to 0.2 and its way better now , slower but its ok
author_association: COLLABORATOR