[ADD] chomp planning files

RMDLO · Aug 8, 2024 · 73012d6 · 73012d6
1 parent 4a88c6c
commit 73012d6
Show file tree

Hide file tree

Showing 7 changed files with 244 additions and 20 deletions.
diff --git a/abb_control/launch/abb_control.launch b/abb_control/launch/abb_control.launch
@@ -64,13 +64,19 @@
       <arg name="publish_monitored_planning_scene" value="true" />
       <arg name="fake_execution" value="true" />
       <arg name="pointer" value="$(arg pointer)"/>
+      <arg name="allow_trajectory_execution" value="true"/>
+      <arg name="info" value="true"/>
+      <arg name="planner" value="ompl" />
     </include>
   </group>
 
   <group unless="$(arg sim)">
     <include file="$(find abb_irb120_moveit_config)/launch/move_group.launch">
       <arg name="publish_monitored_planning_scene" value="true" />
       <arg name="pointer" value="$(arg pointer)"/>
+      <arg name="allow_trajectory_execution" value="true"/>
+      <arg name="info" value="true"/>
+      <arg name="planner" value="ompl" />
     </include>
   </group>
 

diff --git a/abb_irb120_moveit_config/config/chomp_planning.yaml b/abb_irb120_moveit_config/config/chomp_planning.yaml
@@ -0,0 +1,28 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+animate_path: true
+add_randomness: false
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+hmc_discretization: 0.01
+hmc_stochasticity: 0.01
+hmc_annealing_factor: 0.99
+use_hamiltonian_monte_carlo: false
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+animate_endeffector: false
+animate_endeffector_segment: "gripper_tcp_m"
+joint_update_limit: 0.1
+collision_clearence: 0.2
+collision_threshold: 0.07
+random_jump_amount: 1.0
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5
+trajectory_initialization_method: "quintic-spline"
diff --git a/abb_irb120_moveit_config/config/kinematics.yaml b/abb_irb120_moveit_config/config/kinematics.yaml
@@ -1,8 +1,8 @@
 mp:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
+  kinematics_solver_timeout: 1.0
 mp_m:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
+  kinematics_solver_timeout: 1.0
diff --git a/abb_irb120_moveit_config/launch/chomp_planning_pipeline.launch.xml b/abb_irb120_moveit_config/launch/chomp_planning_pipeline.launch.xml
@@ -0,0 +1,21 @@
+<launch>
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters" default="default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints"
+                />
+
+  <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find abb_irb120_moveit_config)/config/chomp_planning.yaml" />
+</launch>
diff --git a/abb_irb120_moveit_config/launch/move_group.launch b/abb_irb120_moveit_config/launch/move_group.launch
@@ -1,5 +1,6 @@
 <launch>
-
+
+  <arg name="planner" default="ompl" />
   <arg name="pointer" default="false" />
 
   <include file="$(find abb_irb120_moveit_config)/launch/planning_context.launch">
@@ -26,7 +27,7 @@
 
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find abb_irb120_moveit_config)/launch/planning_pipeline.launch.xml">
-    <arg name="pipeline" value="ompl" />
+    <arg name="pipeline" value="$(arg planner)" />
   </include>
 
   <!-- Trajectory Execution Functionality -->

diff --git a/abb_irb120_moveit_config/rviz/debug.rviz b/abb_irb120_moveit_config/rviz/debug.rviz
@@ -1,13 +1,16 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /MotionPlanning1
+        - /MotionPlanning1/Planned Path1
+        - /TF1
+        - /TF1/Frames1
       Splitter Ratio: 0.7425600290298462
-    Tree Height: 130
+    Tree Height: 269
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -43,7 +46,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      JointsTab_Use_Radians: false
+      JointsTab_Use_Radians: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -176,7 +179,7 @@ Visualization Manager:
         Robot Color: 150; 50; 150
         Show Robot Collision: true
         Show Robot Visual: true
-        Show Trail: true
+        Show Trail: false
         State Display Time: 0.05 s
         Trail Step Size: 1
         Trajectory Topic: move_group/display_planned_path
@@ -437,6 +440,171 @@ Visualization Manager:
         node_results_node_9: true
       Queue Size: 100
       Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: false
+        base_link_m:
+          Value: false
+        camera:
+          Value: false
+        camera_aligned_depth_to_color_frame:
+          Value: false
+        camera_color_frame:
+          Value: false
+        camera_color_optical_frame:
+          Value: false
+        camera_depth_frame:
+          Value: false
+        camera_depth_optical_frame:
+          Value: false
+        camera_link:
+          Value: false
+        camera_mount:
+          Value: false
+        gripper_base_m:
+          Value: false
+        gripper_m:
+          Value: false
+        gripper_tcp_m:
+          Value: false
+        idx_20:
+          Value: false
+        idx_50:
+          Value: false
+        idx_80:
+          Value: false
+        idx_mid7:
+          Value: false
+        idx_r1_1:
+          Value: false
+        idx_r2_1:
+          Value: false
+        idx_r2_2:
+          Value: false
+        idx_r2_3:
+          Value: false
+        idx_reset_1:
+          Value: true
+        idx_reset_2:
+          Value: true
+        idx_reset_3:
+          Value: true
+        idx_x_1:
+          Value: false
+        idx_x_2:
+          Value: false
+        link_1:
+          Value: false
+        link_1_m:
+          Value: false
+        link_2:
+          Value: false
+        link_2_m:
+          Value: false
+        link_3:
+          Value: false
+        link_3_m:
+          Value: false
+        link_4:
+          Value: false
+        link_4_m:
+          Value: false
+        link_5:
+          Value: false
+        link_5_m:
+          Value: false
+        link_6:
+          Value: false
+        link_6_m:
+          Value: false
+        world:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        camera_link:
+          camera_aligned_depth_to_color_frame:
+            {}
+          camera_color_frame:
+            {}
+          camera_depth_frame:
+            camera_depth_optical_frame:
+              {}
+        world:
+          base_link:
+            link_1:
+              link_2:
+                link_3:
+                  link_4:
+                    link_5:
+                      link_6:
+                        camera:
+                          {}
+                        camera_color_optical_frame:
+                          {}
+                        camera_mount:
+                          {}
+          base_link_m:
+            link_1_m:
+              link_2_m:
+                link_3_m:
+                  link_4_m:
+                    link_5_m:
+                      link_6_m:
+                        gripper_m:
+                          gripper_base_m:
+                            gripper_tcp_m:
+                              {}
+          idx_20:
+            {}
+          idx_50:
+            {}
+          idx_80:
+            {}
+          idx_mid7:
+            {}
+          idx_r1_1:
+            {}
+          idx_r2_1:
+            {}
+          idx_r2_2:
+            {}
+          idx_r2_3:
+            {}
+          idx_reset_1:
+            {}
+          idx_reset_2:
+            {}
+          idx_reset_3:
+            {}
+          idx_x_1:
+            {}
+          idx_x_2:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /knotdlo/initial
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 211; 215; 207
@@ -469,9 +637,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.9747960567474365
+      Pitch: 1.3247957229614258
       Target Frame: base_link
-      Yaw: 1.561761498451233
+      Yaw: 1.5417615175247192
     Saved: ~
 Window Geometry:
   Displays:
@@ -487,7 +655,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000200000000000002bb0000039efc0200000008fb000000100044006900730070006c006100790073000000003d00000113000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000003d000002250000017d00fffffffb0000000a0049006d0061006700650100000268000001730000001600ffffff000000020000047d0000018bfc0100000002fb0000000a0049006d00610067006501000002c1000002d20000007600fffffffb0000000a0049006d0061006700650100000599000001a50000007600ffffff0000047d0000020d00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000002000000000000027c0000039efc0200000008fc0000003d00000167000000e60100001cfa000000000100000003fb0000000a0049006d0061006700650100000000ffffffff0000007600fffffffb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb000000100044006900730070006c0061007900730100000000000002bb0000015600fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000010c0000017d0000017d00fffffffb0000000a0049006d00610067006501000001aa000002310000001600ffffff00000002000004bc0000018bfc0100000002fb0000000a0049006d0061006700650100000282000003100000007600fffffffb0000000a0049006d0061006700650100000598000001a60000007600ffffff000004bc0000020d00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1854

diff --git a/abb_irb120_moveit_config/rviz/record.rviz b/abb_irb120_moveit_config/rviz/record.rviz
@@ -44,6 +44,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
+      JointsTab_Use_Radians: false
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -510,15 +511,14 @@ Visualization Manager:
                               {}
       Update Interval: 0
       Value: true
-    - Class: rviz/InteractiveMarkers
-      Enable Transparency: true
-      Enabled: false
-      Name: InteractiveMarkers
-      Show Axes: false
-      Show Descriptions: true
-      Show Visual Aids: false
-      Update Topic: /interactive_marker_server/update
-      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /abb_control/reachability
+      Name: MarkerArray
+      Namespaces:
+        reachable_points: true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255