moveit takes a lot of time for plan and execute

edit

Hi community, I'm a newbie of ROS and i had started to work with a Yumi robot of ABB (IRB 14000). In order to use ROS Kinetic (with ubuntu 16.04) with this robot i followed these guides and installed these packages Now i'm in some trouble with the trajectory planning mainly because:

1. The trajectory planning takes a lot of time also for very easy poses: for example
2. The planners do "whatever they want" (but probably i fixed with my old question link text)
3. Sometime the planner does not compute the path for no reason

I searched a lot in the web and i had setted/changed different parameters in the ompl_planning.yaml file. For example now i'm using:

  RRTstarkConfigDefault:
type: geometric::RRTstar
termination_condition: Iteration[10]
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

Is there are also a "good way" to program the motion? I followed all the tutorials regarding Python language because is the one that i'm using and the code written by me is:

#!/usr/bin/env python


import copy
from typing import Any
from moveit_msgs.msg import *
from moveit_commander import *
from std_msgs.msg import String
import geometry_msgs.msg
from yumi_utils import PI, gripper_effort
from yumi_hw.srv import *
from tf.transformations import quaternion_from_euler, concatenate_matrices, euler_matrix, quaternion_from_matrix


# Arm IDs
RIGHT = 1  # :ID of the right arm
LEFT = 2  # :ID of the left arm
BOTH = 3  # :ID of both_arms

# Defining the measure of the table
table_height = 0.025  # [m] :The height of the upper surface of the table (z)
table_length = 1.200  # [m] :The width of the table (y)
table_width = 0.400  # [m] :The width of the table (x)

# Length of the Gripper from datasheet
z_gripper = 0.136  # [m]

# Distance of the center of the cam from yumi_link_7_r
z_cam = 0.040  # [m]

# Length of the test tube
length_tube = 0.125  # [m]

# Choice of the planners
planner = "RRTstarkConfigDefault"  # Asymptotic optimal tree-based planner
# planner = "ESTkConfigDefault"  # Default: tree-based planner
# planner = "RRTConnectConfigDefault"  # Tree-based planner
# planner = "PRMstarkConfigDefault"  # Probabilistic Roadmap planner

planning_attempts = 100  # planning attempts
planning_time = 50  # [s] Planning time for computation

# Defining the workspace [min X, min Y, min Z, max X, max Y, max Z]
ws_R = [0.000, -table_length/2, table_height, 0.600, 0.200, 0.593]
ws_L = [0.000, -0.200, table_height, 0.600, table_length, 0.593]

# Initialization of the Node
rospy.init_node('demo_test', anonymous=True, log_level=rospy.DEBUG)
# Initialization of Moveit
rospy.loginfo('Starting the Initialization')
roscpp_initialize(sys.argv)

robot = RobotCommander()
scene = PlanningSceneInterface()
mpr = MotionPlanRequest()
rospy.sleep(1.0)

# Left arm
group_l = MoveGroupCommander("left_arm")
# Type of planner
group_l.set_planner_id(planner)
group_l.set_pose_reference_frame("yumi_body")

# Setting the workspace
group_l.set_workspace(ws=ws_L)

# Replanning
group_l.allow_replanning(True)
group_l.set_goal_tolerance(0.005)
group_l.set_num_planning_attempts(planning_attempts)
group_l.set_planning_time(planning_time)

# Right arm
group_r = MoveGroupCommander("right_arm")
# Type of planner
# group_r.set_planner_id(planner)
group_r.set_pose_reference_frame("yumi_body ...