From 8a1b8130dce6bbb08c7281ea554f411a23eb5d37 Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Wed, 13 Oct 2021 11:59:10 +0200
Subject: [PATCH] Change MoveIt planner

---
 launch/simulation.launch       | 8 +++++---
 src/active_grasp/controller.py | 5 ++++-
 2 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/launch/simulation.launch b/launch/simulation.launch
index 3df0306..a9d12aa 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="launch_rviz" default="false" />
+  <arg name="rviz" default="false" />
 
   <!-- Load parameters -->
   <rosparam command="load" file="$(find active_grasp)/cfg/active_grasp.yaml" subst_value="true" />
@@ -12,8 +12,10 @@
 
   <!-- Launch MoveIt -->
   <node pkg="tf2_ros" type="static_transform_publisher" name="to_panda" args="0 0 0 0 0 0  world panda_link0" />
-  <include file="$(find panda_moveit_config)/launch/move_group.launch" />
+  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="pipeline" value="ompl"/>
+  </include>
 
   <!-- Launch rviz -->
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" if="$(arg launch_rviz)" />
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" />
 </launch>
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index ebd0030..878fdcb 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -51,6 +51,9 @@ class GraspController:
     def init_moveit(self):
         self.moveit = MoveItClient("panda_arm")
         rospy.sleep(1.0)  # Wait for connections to be established.
+        self.moveit.move_group.set_planner_id("PRMstarkConfigDefault")
+        self.moveit.move_group.set_num_planning_attempts(10)
+        self.moveit.move_group.set_planning_time(5.0)
 
     def switch_to_cartesian_velocity_control(self):
         req = SwitchControllerRequest()
@@ -167,7 +170,7 @@ class GraspController:
         self.moveit.goto(T_base_grasp * self.T_grasp_ee)
         self.gripper.grasp()
         self.moveit.goto(Transform.t([0, 0, 0.1]) * T_base_grasp * self.T_grasp_ee)
-        success = self.gripper.read() > 0.005
+        success = self.gripper.read() > 0.002
         return "succeeded" if success else "failed"
 
     def postprocess(self, T_base_grasp):