MoveIt Planning and Execution Fails

edit

Hi,

I am facing some issues with MoveIt. I have a 6 DOF robot and I have created the moveit_config package for it. I have also followed this tutorial and I am able to add objects to the planning_scene and attach them to my robot as well.

However, I am facing problems with the planning and execution in MoveIt. I have followed the Move Group Interface tutorial and I have made some nodes for Joint Space Goals and also Pose Goals. Here is a snippet of my node:

moveit::planning_interface::MoveGroup group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 

std::vector<double> j_values;
j_values.resize(6);
j_values[0] = 0.00;
j_values[1] = -0.80;
j_values[2] = 0.73;
j_values[3] = 0.00;
j_values[4] = 0.755;
j_values[5] = 0.00;
group.setJointValueTarget(j_values);

//Motion plan from current location to custom position
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
ROS_INFO("Visualizing Plan %s",success?"":"FAILED");
/* Sleep to give RViz time to visualize the plan. */
sleep(5.0);

moveit_msgs::MoveItErrorCodes error_codes;
if (success == true)
{
ROS_INFO("Planning To Move");
error_codes = group.move();
ROS_INFO("%d", error_codes.val);
}

I have used the GUI sliders of the joint state publisher and noted these joint values so I am certain that they are valid positions. The funny part is that sometimes the planning fails and sometimes, even if the planning succeeds and I am able to visualize the trajectory in RViz, the fake trajectory execution fails.

I have also seen this behaviour when I set Pose Goals and Cartesian Path Plan.

I have setup MoveIt without any controllers. I am using the fake_arm_controller. Could this have to do with anything?

Also I was reading about KDL and in some places, it is given that it works for >= 6 DOF arms but in the book Mastering ROS for Robotics Programming on page 404, it is mentioned that KDL works well for DOF > 6.

Which one holds true? Should I start looking into IKFast?

Thanks for your inputs!

EDIT :

This is what I get in the terminal from which I launch my node :

[ INFO] [1482329713.395102382]: Loading robot model 'mitsubishi_rv6sd'...
[ INFO] [1482329713.792179428]: Loading robot model 'mitsubishi_rv6sd'...
[ INFO] [1482329714.748339492]: Ready to take MoveGroup commands for group arm.
[ INFO] [1482329718.508234984]: Visualizing Plan 
[ INFO] [1482329723.508560155]: Planning To Move
[ INFO] [1482329728.544356683]: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1482329728.544465716]: -1
terminate called after throwing an instance of 'class_loader::LibraryUnloadException'
  what():  Attempt to unload library that class_loader is unaware of.
Aborted (core dumped)

and this from the move_group terminal :

[ INFO] [1482329714.751108276]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1482329714.784687548]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1482329717.107128883]: LBKPIECE1: Created 184 (156 start + 28 goal) states in 169 cells (151 start (151 on boundary) + 18 goal (18 on boundary))
[ INFO] [1482329717.107195147]: Solution found in 2.333180 seconds
[ INFO] [1482329718.133562849]: SimpleSetup: Path simplification took 1.026287 seconds and changed from 30 to 25 states
[ INFO ...