Problems with generated Moveit config package for custom robot

edit

Hello,

I am trying to move a custom robot in simulation with MoveIt. I work with ROS kinetic on Ubuntu 16.04 in a VM.

I started with the original URDF of the Schunk LWA4P and generated a Moveit configuration package for it with the MoveIt setup assistant. Everything worked as expected and I could move the Schunk LWA4P with interactive markers in rviz.

For my custom robot I modified the URDF of the Schunk LWA4P by adding some obstacles (walls, table), a gripper and a camera. The modified URDF is properly displayed in rviz. Again, I generated a Moveit configuration package with the MoveIt setup assistant for this modified robot. (Hence, I regenerated the collision matrix and added a new planning group for the gripper)

Now when I launch the simulated custom robot and Moveit successfully with:

$ roslaunch schunk_lwa4p sim_with_gripper.launch 
$ roslaunch lwa4p_movit_config moveit_planning_execution.launch

I observe a strange behaviour: It seems that the motion planning works for all movements that don't require to move the first joint arm_1_joint. For instance, simply moving the robot up and down perfectly works, as this does not require the joint arm_1_joint to move at all.

However, if I try to move the robot left or right, planning fails, giving me the following output:

[ INFO] [1574340636.590591270, 370.486000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1574340636.591234451, 370.487000000]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1574340636.592118749, 370.488000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1574340636.592193685, 370.488000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1574340636.592314248, 370.488000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1574340636.592467171, 370.488000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1574340641.591400125, 375.367000000]: RRTConnect: Created 2190 states (1056 start + 1134 goal)
[ INFO] [1574340641.591594623, 375.367000000]: RRTConnect: Created 2238 states (1097 start + 1141 goal)
[ INFO] [1574340641.591739554, 375.367000000]: RRTConnect: Created 2038 states (1016 start + 1022 goal)
[ INFO] [1574340641.591953590, 375.367000000]: RRTConnect: Created 1901 states (904 start + 997 goal)
[ WARN] [1574340641.592145409, 375.368000000]: ParallelPlan::solve(): Unable to find solution by any of the threads in 5.000663 seconds
[ INFO] [1574340641.592256769, 375.368000000]: Unable to solve the planning problem
[ WARN] [1574340641.598533896, 375.373000000]: Fail: ABORTED: No motion plan found. No execution attempted.

or planning fails as a collision is detected in the link arm_2_link for no apparent reason

For me, this seems like a problem either in my URDF or generated SRDF. Do you have any idea of what is going on? I uploaded my MoveIt and simulation packages to github.

Any help is highly appreciated!

This is the output of $ roslaunch lwa4p_movit_config moveit_planning_execution.launch at startup:

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://VMLinux1604Simon:42355/

SUMMARY
========

PARAMETERS ...